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I.  INTRODUCTION 

A.  SPATIAL  REASONING  PHILOSOPHY 

Good  spatial  reasoning  is  critical  to  the  success  of  any  mobile  robotics 
project;  it  allows  a  robot  to  interpret  what  it  "sees"  and  to  perform  intelligent 
motions.  Spatial  reasoning  requires  the  robot  to  create  and  maintain  a  cognitive 
map,  or  knowledge  structure,  of  its  world.  There  are  two  main  schemes 
employed  by  robotics  projects  to  represent  this  cognitive  map:  occupancy  arrays 
and  constructive  solid  geometry. 

The  implementation  of  an  occupancy  array  representation  in  2-D  is  similar 
to  the  implementation  of  a  graphic  picture.  Grids,  similar  to  the  pixels  of  a 
computer  monitor,  divide  the  world.  The  labels,  occupied,  empty  or  unknown, 
similar  to  the  red,  green,  blue  (RGB)  designations  used  in  graphics,  give  the 
status  of  each  grid  Additionally,  each  grid  has  an  uncertainty  factor  between  0 
and  1 ,  similar  to  the  0  -  255  value  assigned  to  the  RGB  colors.  This  uncertainty 
factor  arises  due  to  incomplete  sensor  data  and/or  partially  occupied  grids. 
Humans  can  visualize  occupancy  arrays  easily;  the  occupancy  arrays  easily 
transform  into  a  graphic  picture.  However,  they  do  have  some  drawbacks.  If  an 
object  moves,  the  uncertainty  of  its  position  increases  dramatically. 

Furthermore,  occupancy  arrays  require  a  significant  amount  of  space,  depending 
on  the  granularity  of  the  representation.  For  example,  suppose  the  robot 
requires  2-D  knowledge  of  its  world  to  within  one  centimeter  and  its  world  is  a 
five  meter  square  box.  This  world  is  represented  by  a  1 000  x  1 000  array 
containing  one  million  grids;  Each  element  of  the  array  would  need  to  maintain 
information  on  both  the  status  and  uncertainty  of  the  grid.(Davis,  1990,  pp.  264- 
270)  Many  robotics  projects,  such  as  the  Neptune,  Terregator  and  Uranus 
robots  at  Camegie-Mellon  University,  use  occupancy  arrays  to  describe  their 
robot’s  world  (Elfes,  1 987,  p.  255). 

The  other  common  implementation  of  the  cognitive  map  uses 
Constructive  Solid  Geometry  (CSG).  CSG  represents  complex  objects  as  a 
combination  of  fundamental  shapes.  The  fundamental  shapes  used  depends  on 
the  domain  of  the  world.  Each  shape  has  its  own,  or  local,  coordinate  system 
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and  this  coordinate  system  has  a  position  relative  to  the  world,  or  global, 
coordinate  system.  The  description  of  an  object  consists  of  the  appropriate 
dimensions  and  the  location  and  orientation  of  the  local  coordinate  system  for 
each  fundamental  shape  forming  the  object.  This  works  fine  for  describing  a 
known  world,  but  the  method  breaks  down  when  an  unknown  obstacle  is 
encountered.  The  robot  is  unable  to  determine  neither  the  dimensions  nor  the 
location  of  the  local  coordinate  system  of  each  fundamental  shape  needed  to 
compose  the  object;  the  robot  only  knows  information  about  the  visible  boundary 
of  the  object  and  can  not  determine  to  what  shape  this  boundary  belongs.(Davis, 
1990,  pp.  270-273) 

B.  SENSORS 


Intelligent  management  of  on  board  sensors  is  critical  to  successful  robot 
navigation.  Some  of  the  basic  types  of  sensors  used  in  robotics  include  sonars, 
Charge-Coupled  Device  (CCD)  cameras,  laser  range  finders  and  tactile  sensors. 
Each  sensor  type  has  its  own  set  of  strengths  and  limitations  which  needs  to  be 
considered  when  designing  a  sensor  suite.  The  sensor  suite  should  use 
complementary  sensors  and  should  only  use  those  sensors  which  are 
appropriate  for  the  particular  application.  Correlating  data  from  multiple  sensors 
is  an  extremely  difficult  problem  that  many  organizations  and  universities, 
including  this  one,  are  trying  to  solve.  The  problem  is  complicated  even  further 
when  different  types  of  sensors  provide  the  data.  However,  the  benefits  of  this 


Laser  range  finders  use  the  same  basic  principles  as  ultrasonic  sonars; 
they  measure  the  time  of  flight  to  determine  distance.  However,  since  laser 
range  finders  use  the  speed  of  light  (3  x  108  m/s),  their  response  time  is  much 
better  than  that  of  ultrasonic  sonars  which  use  the  speed  of  sound  (343  m/s). 
They  are  not  used  on  smaller  robotics  projects  because  they  have  a  higher  price 
tag  and  require  more  power. 

Tactile  sensors  are  used  mainly  with  manipulators,  but  have  found  uses 
with  mobile  robots.  They  have  been  used  on  the  feet  of  walking  robots  to 
facilitate  foot  placement  and  on  the  periphery  of  wheeled  robots,  similar  to  curb 
feelers  installed  on  some  cars. 

C.  YAMABICO-11  PHILOSOPHY 

Yamabico-1 1  is  a  research  robot  at  the  Naval  Postgraduate  School  whose 
purpose  is  to  implement  and  validate  new  theories  in  robotics,  including  motion 
control  and  spatial  reasoning.  Consequently,  its  configuration,  both  hardware 
and  software,  is  continually  evolving. 

The  Yamabico-1 1  project  uses  a  method  similar  to  CSG  to  describe  its 
world;  the  world  and  the  objects  in  it  are  depicted  by  polygons.  Known  objects 
are  fitted  to  polygons  located  within  the  inverted  polygon  representing 
Yamabico-1  Ts  world.  As  Yamabico-1 1  moves  within  its  world,  it  uses  a  least- 
squares  fitting  algorithm  to  fit  its  positional  sonar  readings  to  line  segments. 
Yamabico-1 1  matches  these  line  segments  to  its  known  world;  if  no  known  object 
corresponds  to  the  sonar  data,  the  line  segments  are  stored  as  a  new  object. 

This  method  has  advantages  over  occupancy  arrays  and  CSG  because  it 
requires  less  storage  space  and  does  not  need  to  know  information  about  the 
location  and  orientation  of  the  local  coordinate  system  of  objects.  Fitting  sonar 
data  to  line  segments  reduces  the  impact  of  partial  and/or  erroneous  data. 
Objects,  both  known  and  unknown,  are  represented  by  the  global  position  of 
each  vertex  of  the  polygon  or  endpoints  of  the  line  segment. 

Yamabico-1 1  uses  and  array  of  twelve  ultrasonic  sonars  as  its  main 
sensor  system.  Since  Yamabico-1 1  operates  in  a  controlled  environment,  it 
does  not  need  a  sophisticated,  long-range  sensor  system.  Since  Yamabico-1 1 
also  serves  as  a  teaching  tool,  it  includes  a  CCD  camera  to  provide  a  means  for 
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image  processing  research.  Since  image  processing  is  a  CPU  intensive 
operation,  the  CCD  camera  has  not  been  incorporated  as  an  integral  part  of  the 
sensor  suite.  Future  plans  for  Yamabico-1 1  development  include  the  integration 
of  the  CCD  camera. 

The  quality  of  the  data  received  depends  on  the  sensor  system 
characteristics.  This  research  work  examines  the  quality  of  the  data  produced 
by  sonars  and  shows  how  to  use  this  data  for  intelligent  obstacle  avoidance. 


II.  PROBLEM  STATEMENT  AND  APPROACH 


A.  PURPOSE 

The  purpose  of  this  thesis  work  was  to: 

1 .  Determine  the  acoustic  characteristics  of  the  current  ultrasonic  sensors 
and  the  effects  of  the  current  sensor  configuration  on  the  capabilities  and 
limitations  of  Yamabico-1 1.  It  will  explain  the  origin  of  the  observations  made  so 
far  and  will  help  to  develop  corrections  to  the  sensor  data  processing  to  adjust 
for  the  physical  phenomena. 

2.  Improve  the  capability  of  Yamabico-1 1  to  navigate  autonomously 
around  unknown  obstacles. 

B.  APPROACH 

1.  Acoustic  Characteristics 

The  approach  followed  during  this  research  work  was  to: 

a.  Determine  the  theory  of  the  physical  phenomena  related  to  the 
ultrasonic  sensors. 

b.  Test  and  verify  the  theoretical  predictions  in  the  laboratory  setting. 

c.  Make  recommendations  for  improvements. 

d.  Implement  improvements. 

e.  Test  the  improvements  using  Yamabico-1 1  as  the  test  bed. 

2.  Robot  Navigation 

Intelligent  robot  navigation  requires  interfacing  the  sensor  system(s)  and 
the  path  planning  module  to  derive  a  workable  algorithm  for  obstacle  avoidance. 
This  task  was  accomplished  by: 
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a.  Developing  a  dynamic  function  to  determine  a  safe  path  for  avoiding 
an  obstacle,  assuming  either  the  vertices  of  the  object  are  known  or 
the  width  can  be  determined. 

b.  Testing  the  ability  of  Yamabico-1 1  to  avoid  obstacles  autonomously 

using  this  function  in  a  variety  of  scenarios. 
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III.  YAMABICO-11 

A.  HISTORY 

Yamabico-1 1  is  an  autonomous  mobile  robot  powered  by  two  12-volt 
batteries  and  driven  on  two  wheels  by  DC  motors.  These  motors  drive  and 
steer  the  wheels  while  four  shock  absorbing  caster  wheels  balance  the  robot.  It 
uses  twelve  40  kHz  ultrasonic  sensors  to  sense  its  environment.  Recently,  its 
master  processor  was  upgraded  from  the  Motorola  MC68020  microprocessor  to 
the  SPARC4  microprocessor.  This  upgrade  to  the  SPARC4  microprocessor 
expanded  Yamabico-1  Ts  memory  and  changed  the  development  environment. 
The  high-level  Model-based  Mobile-robot  Language  (MML)  software,  written  in 
ANSI  C,  is  compiled  using  a  "Makefile"  and  then  downloaded  to  Yamabico-1 1 . 
An  onboard  laptop  console  (Macintosh  145  Powerbook)  provides  real-time 
command  level  communication  between  the  user  and  the  robot. 

The  Motorola  microprocessor  on  Yamabico-1 1  uses  MML  Version  1 0 
(MML  10)  and  requires  compilation  using  the  "gravyS"  server  in  the  Computer 
Science  Department  at  the  Naval  Postgraduate  School.  MML  10  consists  of  a 
kernel  and  a  user  program.  The  kernel  contains  the  compiled  code  for  the 
robot's  application  software.  The  user  program  uses  the  MML  functions  to 
control  the  robot.  Once  compiled,  these  programs  are  downloaded  to  the  robot 
via  an  RS-232  link  at  a  baud  rate  of  19,200.  Typing  the  command  "Io=dluk"  at 
the  prompt  on  the  laptop  downloads  both  the  kernel  and  the  user  program  to 
Yamabico-1 1 ;  typing  "lo=dlu"  downloads  just  the  user  program.  Once 
downloaded,  the  user  types  "g  304000"  to  run  the  user  program. 

The  SPARC4  microprocessor  on  Yamabico-1 1  uses  MML  Version  1 1 
(MML  11).  Compilation  of  MML  1 1  uses  the  GNU  'C  Compiler  available  on  the 
SPARC  workstations  and  downloading  uses  an  Ethernet  cable  connected  to  the 
"libra"  server  in  the  Computer  Science  Department  at  the  Naval  Postgraduate 
School.  The  "Makefile"  in  MML  1 1  compiles  the  kernel  files  and  the  user  files 
into  a  single  executable  file  called  "user"  and  copies  this  file  into  the 
"SPARC4/target"  directory  in  the  "yamabico"  account.  The  "libra"  server 
automatically  checks  this  directory  every  minute  to  see  if  the  user  program  has 
changed  and  updates  its  files  accordingly.  This  step  is  required  to  ensure  the 
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S6curity  3nd  intsQrity  of  tho  "librs"  sorvor.  To  lo3d  th©  6X6cut3bl8  proQrsm,  th© 
us©r  types  "bootp"  st  the  prompt  on  the  leptop.  If  the  user  enters  "bootp"  3t  the 
Isptop  prompt  before  the  "libra"  server  h3s  upd3ted  its  files,  the  new  compiled 
program  will  not  be  available  yet,  so  the  old  compiled  program  will  be  loaded. 
Therefore,  it  is  important  that  the  user  wait  at  least  one  minute  after  compiling 
the  program  before  downloading  the  program  to  Yamabico-1 1 .  Once  loaded,  the 
user  types  "run"  to  execute  the  user  program. 

Navy  Lieutenant  Scott  Book  developed  MML  1 1  in  March  1 994.  MML  1 0 
became  cumbersome  with  the  addition  of  new  functions.  MML  10  relied  heavily 
upon  numerous  global  variables  and  did  not  adhere  to  standard  software 
engineering  practices.  Although  MML  1 1  allows  Yamabico-1 1  to  use  the 
SPARC4  microprocessor,  the  main  thrust  of  Lieutenant  Book's  work  was  the 
restructuring  of  the  MML  to  eliminate  global  variables  and  secondly  to  develop 
guidelines  for  future  programming.  Lieutenant  Book  successfully  implemented 
and  tested  the  motion  control  functions  in  MML  11.  In  September  1994,  Navy 
Lieutenant  Commander  Frank  Kelbe  converted  the  sonar  functions  from  MML  1 0 
to  MML  11. 


B.  SONAR  SYSTEM 

1.  Hardware  System 


Yamabico-1  Ts  sonar  system  has  been  evolving  since  1980.  The  original 


a.  Sensor  Configuration 


In  the  original  design,  the  twelve  ultrasonic  sonar  pairs  were 
divided  into  three  logic  control  groups  having  each  of  their  sensors  located  at  90 
degree  angles  from  each  other;  group  0  consists  of  pairs  0,  2,  5  and  7;  group  1 
consists  of  pairs  1 ,  3,  4  and  6;  and  group  2  consists  of  pairs  8,  9,  1 0  and  1 1 . 

This  grouping  allowed  four  ultrasonic  sonar  pairs  to  operate  simultaneously 
without  interference.  (Sherfey.  1 991 ,  pp.  10-11)  Additionally,  the  sonar  pairs 


4 

5 


1  2 

Figure  1 

Ultrasonic  Sensor  Pair  Location 

were  physically  grouped  in  order  to  distribute  the  electrical  load  over  the  driver 
boards  evenly.  Sonar  pairs  0,  2,  8  and  1 1  were  on  sonar  driver  board  1 ;  sonar 
pairs  4,  6,  7  and  5  run  off  of  sonar  driver  board  2  while  sonar  pairs  1 ,  3,  9  and  1 0 
work  from  sonar  driver  board  3.  Figure  2  shows  the  relationship  between  the 
different  sonar  pairs,  the  sonar  motherboard  and  Yamabico-1  Ts  CPU. 
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c.  Sonar  Range  Calculation 


The  minimum  range  was  based  on  the  receiver  being  disabled 
during  the  transmit  pulse.  This  prevented  the  receiver  from  being  triggered  by 
crosstalk  from  the  transmitted  signal.  The  pulse  was  transmitted  for  0.5 
milliseconds;  at  343  meters  per  second,  sound  traveled  a  total  of  17.15 
centimeters  in  this  time,  but  since  this  represented  the  two-way  travel  distance, 
the  minimum  theoretical  range  was  one-half  of  this  number  or  8.575  centimeters. 
However,  additional  time  was  needed  to  accommodate  circuitry  switching  and 
settling;  therefore,  in  practice,  firmware  set  the  minimum  range  at  9.6 
centimeters  (Sherfey,  1991,  p.  12). 

The  maximum  range  was  a  function  of  the  hardware  design.  The 
maximum  number  that  could  be  represented  by  12  bits  was  2‘^  - 1  =  4095.  At  6 
microseconds  per  clock  tick,  this  equated  to  (6.0x10“®)  x  (4095)  =  0.02457 
seconds.  In  24.57  milliseconds,  a  sound  wave  could  travel  8.428  meters.  Since 
the  sound  wave  must  travel  both  out  and  back,  the  maximum  one-way  distance 
was  one-half  of  this  distance,  or  4.214  meters.  Therefore,  due  to  system 
configuration  constraints,  Yamabico-1  Ts  sensors  had  an  operating  range  of  9.6 
centimeters  to  4.214  meters.  (Sherfey,  1991,  pp.  11-13) 

d.  Sonar  Driver  Board 

Within  each  logical  group,  two  sensors  were  driven  by  one  driver 
board  while  the  other  two  sensors  were  driven  by  another  driver  board.  The 
driver  board  produced  a  0.5  millisecond  transmit  signal  consisting  of  20  cycles  of 
a  40  kHz,  4.5  V  peak-to-peak  square  wave.  The  driver  board  produced  two 
signals,  TP1  *  and  TP2*.  TP2*  lagged  TP1  *  by  1 80  degrees,  allowing  the  driver 
board  to  power  only  one  of  the  sensors  at  a  time.  The  sonar  control  board 
interrupted  Yamabico-1  Ts  central  processing  unit  only  when  data  was  available 
from  the  sonar  array. 

The  received  signal  was  considerably  weaker  than  the  transmitted 
pulse  so  it  was  sent  through  a  two-stage  amplification  circuit  and  then  to  a 
74LS14  Schmitt  Trigger  which,  given  a  variable  input  voltage,  produced  a 
constant  output  voltage  signal.  However,  the  Schmitt  Trigger  required  a 
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minimum  of  1 .4  Volts  to  operate  (Michiue,  1 994,  p.  32).  The  first  stage  of  the 
amplification  circuit  yielded  a  voltage  gain  of  40  dB  and  the  second  stage  a 
12.87  dB  gain  for  a  total  gain  of  52.87  dB  or  440  (Michiue,  1994,  p.  23). 
Therefore,  a  minimum  6  mV  peak-to-peak  signal  at  the  receiver  was  required  to 
recognize  the  return  signal. 

The  actual  transmitted  pulse  was  a  packet  of  20  individual  pulses 
of  equal  amplitude,  separated  by  25  microseconds.  Because  of  the  finite 
bandwidth  of  the  receiver  transducer  and  preamplifier,  the  output  amplitude  of 
the  receiver  circuitry  increased  linearly  to  reach  a  maximum  according  to  the 
equation 

V 

V  =  — (Eq.  3-1) 

5x10^ 

where  t  is  the  time,  V  is  amplitude  of  the  received  voltage  and  V^ax 
maximum  voltage  reached.  The  output  reached  a  maximum  at  t  =  0.5 
milliseconds  and  then  fell  off  according  to  the  equation 

V  =  V 

^  ''max® 

where  V  is  the  voltage  at  the  receiver,  Vmax  maximum  voltage  reached, 

T  =  0.5  milliseconds  and  t  is  the  time.  Figure  3  gives  a  representation  of  the  two 
pulse  packets.  The  Schmitt  Trigger  fired  once  the  amplitude  of  the  received 
signal  after  amplification,  V,  reached  1 .4  Volts.  The  time  that  this  took  varied 
because  the  maximum  received  signal  strength,  ®  function  of  both 

the  distance  to  and  the  reflectance  of  the  object  ensonified. 


The  clock  counter  -  distance  conversion  algorithm  should  account 
for  the  hardware  side  effects.  At  room  temperature  (20°C)  the  speed  of  sound  in 
air  is  343  m/s.  Using  this  value,  the  delay  in  starting  the  clock  meant  that  the 
true  range  was  about  0.43  or  0.64  centimeters  longer,  depending  on  which 
signal,  TPI*'  or  TP2^  was  used  to  drive  the  transmitter.  Since  the  last  clock 
counter  value  was  copied  to  the  data  register  upon  the  receipt  of  a  return  signal, 
the  value  could  be  as  much  as  one  clock  cycle,  or  6  microseconds,  off,  causing 
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T  ra  nsmit 


Receive 


Figure  3 

Representative  Pulse  Packets 

the  true  range  to  be  longer  yet  by  up  to  another  0.1029  centimeters.  Normally, 
one  assumed  that  the  actual  object  was  ensonified  by  the  leading  edge  of  the 
transmit  pulse.  However,  when  using  a  detection  threshold  and  the  Schmitt 
Trigger,  detection  could  occur  anywhere  within  the  received  pulse.  If  the 
maximum  amplitude  of  the  received  pulse  was  not  detected,  it  was  unlikely  that 
any  of  the  remaining  received  pulses  would  be  detected.  The  detection  could  be 
delayed  for  up  to  0.5  milliseconds,  the  transmit  pulse  width.  This  detection  delay 
equated  to  an  addition  of  up  to  8.575  centimeters  to  the  true  range.  Therefore, 
the  worst-case  accuracy  of  the  sonar  was  the  sum  of  the  delays  or  about  9.1 
centimeters. 

f.  Upgrades 

Although  the  maximum  theoretical  range  was  set  at  4.214  meters 
by  the  register  size,  in  practice,  the  maximum  achievable  range  was  much  less 
due  to  the  sensitivity  of  the  old  sensors  and  circuitry.  The  front  sensors  were 
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upgraded  in  1 993  in  hopes  of  overcoming  the  detection  problems  at  long  ranges 
The  new  sensors  are  the  Nicera  T40-16  transmitter  and  the  Nicera  R40-16 
receiver.  The  physical  diameter  of  these  sensors  was  16.2  millimeters  and  they 
had  an  internal  aperture  diameter  of  7.0  millimeters. 

However,  these  sensors  still  were  being  driven  by  a  5  volt  peak-to- 
peak  supply  voltage  which  produced  an  output  voltage  of  about  4  volts  peak-to- 
peak.  At  long  ranges,  the  received  signal  often  fell  below  the  6  mV  required  to 
fire  the  Schmitt  Trigger,  making  detection  nearly  impossible.  Consequently,  in 
June  1994,  the  supply  voltage  to  the  transmitters  was  increased  from  5  to  12 
volts  peak-to-peak  to  improve  the  capability  of  the  new  sensor  system  to  detect 
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Figure  4 

Sonar  Driver  Board  Configuration 


coordinate  system,  represented  by  the  jt and  y  axes,  and  its  global  coordinate 
system  denoted  by  the  X  and  Y  axes. 

Polygonal  vertices  in  the  global  coordinate  system  describe  the 
boundaries  of  the  world  and  known  objects.  Sonar  returns  are  depicted  in 
Yamabico-11's  local  coordinate  system,  then  transformed  into  the  global 
coordinates.  Once  transformed,  a  linear  fit  to  the  sonar  return  allows  a 
comparison  with  the  objects  in  the  known  world  to  determine  and/or  verify  the 
location  of  Yamabico-1 1 .  Additionally,  the  sonar  system  detects  objects  in  the 
robot's  path,  but  localization  of  these  objects  has  not  yet  been  accomplished. 

The  user  had  to  tell  the  robot  how  and  when  to  use  its  sonar  through  the 
user  program  developed  in  the  MML.  The  user  could  enable/disable  each  of  the 
twelve  sonar  pairs  individually  and  could  indicate  the  desired  type  of  sonar 
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Figure  5 


Mnemonic 

Sonar 

Group 

FRONTL 

0 

0 

FRONTR 

3 

1 

LEFTF 

4 

1 

LEFTS 

5 

0 

RIGHTF 

7 

0 

RIGHTS 

6 

1 

BACKL 

1 

1 

BACKR 

2 

0 

BACKLEFT 

8 

2 

BACKRIGHT 

9 

2 

FRONTRIGHT 

10 

2 

FRONTLEFT 

11 

2 

Table  1 

Old  Sonar  Mnemonics 


In  general,  the  functionality  of  the  sonar  control  code  remained  the  same 
in  MML  1 1 .  However,  function  names  differed  slightly  to  adhere  to  the  new 
function  naming  convention.  The  new  convention  eliminated  the  use  of  the 
underscore  character  ( _ )  to  separate  words  within  a  function  name.  Instead 
MML  1 1  used  a  combination  of  upper-  and  lower-case  letters.  Initially,  sonar 
mnemonics  also  remained  the  same.  For  example,  under  MML  1 1 ,  user  program 
code  may  have  contained  the  following: 

EnableSonar(FRONTR);  /*  Turn  on  sonar  3  */ 

DisableSonar(FRONTL);  /*  Turn  off  sonar  0  */ 

LogSonar  Data(FRONTR);  /*  Begin  logging  data  from  sonar  3  7 

In  addition.  Lieutenant  Commander  Kelbe  converted  the  sonar  data  logging 
functions  in  MML  1 1  to  reuse  code  from  the  motion  data  logging  functions 
previously  implemented. 
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3.  Acoustic  Characteristics 

The  angular  beam  pattern  of  an  ultrasonic  sensor  is  a  critical  parameter. 
Sherfey  reported  that  the  beam  width  of  the  major  lobe  was  determined  by  using 
the  accepted  far-field  approximation  of 

^=1.22—  (Eq.  3-3) 

D 

where  X.is  the  wavelength,  D  is  the  diameter  of  the  uniform  circular  aperture  and 
9  is  the  beam  width  in  radians.  Sherfey  reported  that  for  a  40  kHz  signal  and  an 
ultrasonic  sensor  diameter  of  1 .5  centimeters,  the  acoustic  wave  length  was  8.5 
millimeters  and  produced  a  theoretical  beam  width  of  40°.  In  hopes  of  reducing 
this  beam  width,  the  original  design  placed  cones  around  the  transducers  as 
shown  in  Figure  6.  (Sherfey,  1991,  p.  51)  From  this  geometry,  Sherfey  reported 
that  the  effective  beam  width  at  a  distance  of  one  meter  was  2.6°.  (Sherfey,  p. 
53.) 

Additionally,  Byrne  conducted  experiments  in  1993  to  investigated  the 
data  returned  from  the  sonar  system.  He  determined  that  Yamabico-1 1  could 
map  out  a  straight  wall  accurately  while  performing  either  translational  or 
rotational  movement,  but  that  the  data  returned  from  corners  or  round  objects 
was  sketchy  and  erroneous.  (Byrne,  pp.  29-43)  In  particular,  Byrne  observed 
that  Yamabico-1 1  could  not  map  either  a  concave  or  a  convex  90  degree  angle 
accurately. 

Yamabico-1 1  plots  concave  right  angle  walls  as  a  series  of  short  askew 
line  segments  rather  than  one  continuous  line  segment.  The  linear-fitting 
technique  breaks  down  because  of  sides  lobes  which  cause  improper  distances 
to  be  measured.  Additionally,  Yamabico-1 1  plots  a  line  of  sonar  returns  tangent 
to  the  vertex  of  the  concave  right  angle.  For  the  convex  right  angle,  Yamabico- 
1 1  fails  to  get  usable  sonar  returns.  These  problems  will  be  explained  in 
Chapter  IV  and  are  not  a  failure  of  Yamabico-1 1 ,  but  a  limitation  imposed  by  the 
physical  nature  of  the  problem. 
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IV.  ACOUSTIC  THEORY 


A.  BEAM  WIDTH 


The  term  "beam  width"  is  used  to  describe  the  pressure  field  radiated  by 
an  energy  source.  It  refers  to  the  extremity  of  the  major  lobe  of  the  radiation 
pattern.  Sound  levels  received  off-axis  are  weaker  than  those  received  on  the 
centerline  of  the  beam.  Beam  width  can  refer  to  one  of  several  angles;  the  user 
must  know  which  angle  is  being  referenced.  The  ratio. 


P{e) 


(Eq.  4-1) 


where  P(6)  refers  to  the  off-axis  pressure  and  refers  to  the  centerline  or 
axial  pressure,  is  used  to  define  the  beam  width  angle.  The  common  angles 
used  are  the  half-power  beam  width,  the  half-amplitude  beam  width,  and  the 
nodal  beam  width.  The  half-power  beam  width,  also  referred  to  as  -3  dB  beam 
width,  is  the  angle  at  which  this  ratio  equals  0.707;  the  half-amplitude,  or  -6  dB, 
beam  width  occurs  when  the  ratio  is  0.5;  and  the  nodal  beam  width  takes  place 
when  the  ratio  goes  to  zero.  Figure  7  shows  the  relationship  of  these  different 
beam  widths  for  an  acoustically  simple  source  with  a  circular  aperture. 

Therefore,  one  must  indicate  at  which  point  a  given  beam  width  was  taken. 

Determining  the  beanri  patterns  of  acoustic  sources  is  greatly  simplified  if 
they  can  be  treated  as  simple  sources.  "A  simple  source  is  a  closed  surface, 
vibrating  with  arbitrary  velocity  distribution,  but  of  such  a  size  that  all  dimensions 
are  much  smaller  than  the  wavelength  of  the  emitted  sound  (Kinsler,  1982,  p. 

1 64)."  This  is  not  the  case  for  the  sensors  on  Yamabico-1 1 .  The  wavelength  of 
the  sound  and  the  diameter  of  the  transducer  are  almost  equivalent.  Hence,  the 
transducer  is  a  complex  source. 

Equation  3-3  represents  the  Fraunhofer  diffraction  of  light  through  a 
circular  aperture  and  represents  the  half  angle  from  the  central  peak  to  the  first 
node.  It  assumes  that  the  angle  subtended  is  small,  using  the  small  angle 
approximation  of  0.  If  these  angles  are  not  small,  then  the  correct  expression 

sin  6=  1.22—  (Eq.  4-2) 

D 
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must  be  used.  Equation  3-3  is  an  excellent  approximation  in  optics  becau! 
wavelength  of  light,  A,  is  usually  much  smaller  than  the  diameter  of  the  ap€ 

(A  «  D).  It  also  assumes  that  the  observation  point  is  at  a  sufficient  distan 
from  the  circular  aperture  that  the  light  can  be  approximated  by  plane  wav< 

The  diffraction  pattern  produced  is  a  central  disk,  known  as  the  Airy  disk, 
surrounded  by  concentric  rings.  The  half  angle  subtended  by  the  Airy  disk  is  the 
angle  0  in  Equation  3-3.  This  is  the  half-angle  of  the  first  node. 

However,  in  acoustics,  A  is  often  of  the  same  order  of  magnitude  as  D. 
Therefore,  the  angles  are  not  necessarily  small  and  substituting  9  for  sinO  is  an 
invalid  approximation;  hence.  Equation  4-2  is  the  correct  equation.  The  s 
under  investigation  have  an  aperture  diameter,  D  =  7.0  mm  and  acoustic 
wavelength,  A  =  8.575  mm.  Using  the  optical  approximation.  Equation  3-3 
indicates  that  a  node  occurs  at  about  85.6  degrees.  Equation  4-2  gives  a 
real  solution  of  sin 9=  1 .4945,  indicating  that  there  is  no  nodal  surface. 


The  initial  assumption  is  that  the  source  acts  like  a  uniform  circular  piston 
mounted  in  a  rigid  baffle.  The  beam  pattern  function  for  this  source  is  given  by 

=  (Eq.4-3>‘ 

Aw  sin  y 

where  k  is  the  wave  number,  a  is  the  radius  of  the  source  aperture,  is  the 
first-order  Bessel  function  and  ^is  the  angle  measured  relative  to  the  piston 
axis.  The  wave  number  k  -  2;tf/c  where  f  is  the  frequency  and  c  is  the  speed  of 
sound.  Figure  8  plots  the  functional  behavior  of  2J^(x)/x.  The  sound  pressure 
goes  to  zero  when  the  plot  in  Figure  8  crosses  the  x-axis.  This  occurs  when 

(Eq.  4-4) 

where  is  off-axis  angle  of  the  node,  and  =  3.83  is  the  value  where  the  J, 
Bessel  function  goes  to  zero.  For  a  frequency  of  f  =  40  kHz,  speed  of  sound  in 
air  of  c  =  343  m/s  and  aperture  radius  a  =  3.5  mm,  the  term,  ka,  equals  2.565 
and  the  first  zero  of  the  Bessel  Function  occurs  at  sin$i  =  1 .4945.  Solving  for 
gives  a  non-real  solution,  indicating  that  no  nodal  surface  occurs  within  the  180 
degree  span.  This  result  agrees  with  Olson  who  stated  that  a  nodal  surface  will 
first  appear  for  plane  circular  piston  sources  when  the  ratio  of  aperture  diameter 
to  wavelength  of  sound,  D/A,  is  greater  than  or  equal  to  1 .25  (Olson,  1947,  p. 

39).  For  the  transducer  under  investigation,  D/A  =  0.816;  therefore,  no  nodal 
surface  was  expected. 

The  sound  pressure  ratio  from  Equation  4-1  is  the  same  as  the  ratio 
2J,(x)/x;  from  Figure  8,  the  ratio  2J^{x)/x  equals  0.5  when  x  «  2.2.  Since  x  = 

ka  Sind,  this  ratio  occurs  when  59  degrees,  which  once  again  agrees  with 
Olson.  The  full  beam  width  is  twice  this  angle  or  1 18  degrees.  Since  the 
technical  data  supplied  with  the  transducers  lists  the  half-amplitude  beam  width 
as  50  degrees,  the  assumption  that  the  source  alone  acts  like  a  plane  piston  is 
invalid. 

A  more  probable  assumption  is  that  the  source  acts  like  a  spherical 
source  and  the  casing  acts  as  a  tube  to  produce  a  plane  wave  at  the  output  of 
the  sensor  casing,  imitating  a  circular  piston  source  with  radius  equal  to  that  of 
the  case  opening.  The  analytical  solution  to  this  problem  is  very  complex  due  to 
the  diffraction  of  the  beam  around  the  edges  of  the  tube.  In  an  infinite  baffle,  the 
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Figure  9 

Theoretical  Beam  Pattern  of  Transducer  with  a  =  6.5  mm  and  X  =  8.575  mm 


B.  HORNS 

The  main  purpose  of  a  horn  is  to  increase  the  acoustical  output  of  a 
piston-like  transducer  at  low  frequencies;  increased  directionality  is  a  by¬ 
product.  The  horn  acts  as  an  acoustical  transformer;  it  matches  the  impedance 
of  air  to  that  of  the  piston.  At  low  frequencies,  the  acoustical  impedance  at  the 
throat  of  the  horn  is  greater  than  that  which  would  act  on  a  piston  of  equal  area 
vibrating  in  an  infinite  baffle,  resulting  in  a  greater  acoustical  output.  At  high 
frequencies,  the  horn  has  little  effect.  At  high  frequencies  the  transmitted  beam 
is  much  narrower;  the  horn  does  not  increase  the  acoustical  impedance. 

If  the  wavelength  of  sound  is  greater  than  the  diameter  of  the  horn  mouth, 
then  the  directional  characteristics  will  be  determined  by  the  mouth;  otherwise,  it 
is  the  flare  of  the  horn  which  determines  its  directional  characteristics.  At  high 
frequencies,  the  wavelength  is  small  so  flare  is  important.  The  most  effective 
horn  is  one  in  which  the  rate  of  flare  increases  from  throat  to  mouth. 
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Hyperbolas,  catenaries  and  exponential  functions  have  all  been  used  to 
determine  the  flare  rate.  The  most  commonly  used  horn  employs  the 
exponential  function 

5,  =  S,e’^  (Eq.  4-6) 

where  S,  is  the  cross-sectional  area  at  any  given  position  x,  So  is  the  cross- 
sectional  area  of  the  throat  given  by  So=  where  r  is  the  physical  radius  of  the 
transducer  element,  and  m  is  the  flare  constant.  (Kinsler,  1982,  p.373) 

However,  the  horns  used  on  Yamabico-1 1  are  of  the  simpler  conical  shape 
where 

s,  =  V' 

The  size  of  the  conical  horn  is  important  in  determining  the  beam 
characteristics  it  will  produce.  At  low  frequencies,  where  the  wavelength  of 
sound  is  greater  than  the  diameter  of  the  horn  mouth,  the  pattern  is  the  same  as 
that  produced  by  a  piston  of  the  same  size  as  the  mouth  of  the  horn.  The 
acoustic  waves  exiting  the  mouth  are  essentially  planar.  At  higher  frequencies, 
the  pattern  becomes  narrower  until  it  crosses  over  a  threshold,  at  which  point  the 
exiting  acoustic  waves  are  no  longer  planar.  At  even  higher  frequencies,  the 
circular  conical  horn  acts  the  same  as  a  spherical  surface  source  whose  radius 
is  equal  to  the  distance  as  measured  along  the  side  of  the  horn  from  the 
imaginary  apex  to  the  mouth  opening.  The  exiting  acoustic  waves  are  now 
spherical.  As  the  frequency  continues  to  inaease,  the  pattern  begins  to  broaden 
out  again.  (Olson,  pp.  42-43) 

Yamabico-1 1  uses  two  different  sized  conical  cones  with  the  ultrasonic 
transducers  mounted  inside.  Both  cones  have  a  radius  of  9.25  mm  at  the  point 
where  the  transducers  are  mounted;  this  is  the  throat  radius.  The  small  cone  on 
the  transmitter  has  an  angle  of  opening,  9^,  of  23.7®,  with  a  mouth  radius  of 
1 3.25  millimeters  or  about  1 .56A,  and  overall  length  of  63.20  millimeters  or  about 
7.4  wavelengths  from  the  transducer.  The  larger  cone  on  the  receiver  has  a  9^ 
of  28°,  a  mouth  radius  of  23.75  millimeters  which  is  about  2.QX  and  overall  length 
of  95.26  millimeters  or  about  1 1 .2  wavelengths  from  the  transducer. 
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As  has  been  stated  previously,  the  acoustic  waves  exiting  the  bare 
transducer  are  planar;  therefore,  the  bare  transducer  is  already  producing  the 
desired  beam  pattern.  The  addition  of  a  cone  introduces  impedance 
mismatches  at  both  the  throat  and  mouth  of  the  cone.  These  mismatches  cause 
a  reflected  wave  back  towards  the  transducer,  setting  up  interference.  Hence, 
the  cones  do  not  enhance  the  acoustical  properties  of  the  transducers. 

C.  REFLECTION 

Using  only  a  sonar  system,  a  robot  is  unable  to  map  convex  and  concave 
right  angles  accurately.  The  explanation  for  this  can  be  found  in  basic  physics 
principles.  Detecting  a  convex  right  angle  is  virtually  impossible  given  the 
configuration  in  Figure  10.  The  transmitted  sound  is  reflected  off  the  wall,  away 


Figure  1 0 

Convex  Right  Angle  Detection  and  Mapping 


from  the  robot.  The  angle  of  reflection,  0^,  equals  the  angle  of  incidence,  0^.  In 
Figure  5,  0^  =  45°;  therefore,  the  signal  will  be  reflected  off  of  the  wall  at  45°. 
When  the  robot  is  directly  abreast  of  the  corner  as  in  Figure  1 0,  some  of  the 
signal  will  be  reflected  from  the  very  tip  of  the  comer,  but  this  reflected  signal  is 
very  weak.  Only  a  very  small  percentage,  less  than  one  percent,  of  the 
transmitted  beam  impinges  on  the  corner  tip  in  a  manner  so  it  may  reflect  back 
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to  the  robot.  Since  the  percentage  of  signal  returned  is  so  low,  it  is  then  lost  in 
the  circuit  noise. 

The  concave  right  angle  is  easier  to  detect  although  the  physical  nature 
of  the  problem  makes  it  difficult  to  map  the  concave  right  angle  accurately.  The 
distances  measured  in  the  vicinity  of  the  angle  are  longer  than  they  actually  are. 
Figure  1 1  shows  a  line  segment  extracted  from  the  sonar  return  in  the  vicinity  of 
the  concave  right  angle.  As  expected,  the  line  segment  is  tangent  to  the  vertex. 


Legend:  Distances  as  mapped 

by  Yamabico-1 1 

Figure  1 1 

Concave  Right  Angle  Detection  and  Mapping 

A  concave  right  angle  acts  as  a  retro-reflector;  the  reflected  signal  is  parallel  to 
the  transmitted  signal.  The  path  length  (time)  measured  by  the  sonar  system  is 
longer  than  the  actual  distance.  Figure  12  blows  up  the  corner  and  shows  what 
is  happening.  The  distance  plotted  is  based  on  the  total  path  length  vice  the 
actual  distance  to  the  wall.  The  distance  plotted  is 


X  ~  — {d\ +^/3) 


(Eq.  4-8) 


As  long  as  the  receiver  is  within  the  reflected  beam,  it  will  measure  this  distance. 
The  size  of  the  beam  when  it  reaches  the  wall  depends  on  the  beam  width  and 
distance  to  the  wall.  The  beam  radius  is 

r  =  dxm9  (Eq.  4-9) 

where  d  is  the  distance  to  the  wall  and  ^is  the  half-amplitude  beam  width.  Using 
the  half-amplitude  beam  width  of  27.51°  calculated  for  the  sensors  used  on 
Yamabico-1 1  in  Section  A  above,  the  diameter  of  the  reflected  signal  is  about 
equal  to  the  distance  to  the  wall.  For  all  practical  purposes,  the  receiver  will  be 
within  the  reflected  beam. 


. X . 

Legend:  Distances  as  mapped 

by  Yamabico-1 1 


Figure  12 

Reflection  Plotted  by  Yamabico-1 1 

Although  the  convex  right  angle  can  not  be  mapped  given  the  situation  depicted 
in  Figure  1 0,  the  concave  right  angle  can  be  mapped  accurately  under  the 
situation  in  Figure  11.  Although  Figure  12  shows  61  =  62  =  45°,  the  same  line 
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would  occur  for  01  ^  02.  However,  the  corrections  needed  rely  on  the  fact  that 
the  robot  knows  that  it  is  encountering  a  concave  right  angle.  This  type  of 
knowledge  would  only  exist  if  the  robot  already  has  a  map  of  its  world  and  is  just 
verifying  its  position  within  that  map.  The  corrections  could  not  be  applied  to 
sonar  returns  resulting  from  the  detection  of  an  unknown  object. 
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V.  ANECHOIC  CHAMBER  EXPERIMENTS 

A.  BEAM  PATTERNS 

The  first  experiment  conducted  was  to  verify  the  beam  pattern  of  the 
Nicera  transducer  elements.  Figure  13  shows  the  experiment  setup  used  to 
verify  the  beam  width.  The  technical  data  indicates  that  this  sensor  operates  at 
a  frequency  of  40  kHz,  has  a  -6dB  half-amplitude  beam  width  of  50°,  and  can 
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handle  inputs  up  to  20  Vrms.  The  transmitter  and  receiver  were  mounted  in  a 
manner  to  ensure  that  they  were  operating  in  the  far-field.  Figure  14  shows  the 
actual  setup  in  the  anechoic  chamber  used  for  this  first  experiment. 


Figure  14 

Experiment  1  Setup  in  the  Anechoic  Chamber 

The  voltage  used  to  conduct  these  experiments  was  4.5  Volts  peak-to-peak  as 
this  was  the  effective  voltage  used  by  Yamabico-1 1 .  This  preliminary 
experiment  was  necessary  to  validate  the  experimental  procedures  which  wouk 
be  used  later  on.  Ideally,  the  experiment  would  have  been  conducted  using  an 
omnidirectional  transmitter  and  receiver  to  measure  the  response  of  the  subject 
receiver  and  transmitter,  respectively.  However,  an  omnidirectional  receiver  or 
transmitter  which  operated  at  40  kHz  was  not  available.  Therefore,  the 
experiment  would  have  to  be  conducted  using  an  "identical"  transmitter  and 
receiver. 

Figure  15  shows  the  linear  data  extracted  during  this  experiment.  Figure  16 
depicts  this  information  in  polar  form.  The  theory  presented  in  Chapter  IV  had 
predicted  that  the  node  would  occur  at  52.9°.  Experimentally,  the  node  was 
measured  at  about  58°;  a  difference  of  less  than  10%.  The  -6dB  value  for  the 
half-amplitude  comes  from  the  equation 
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Angle  (degrees) 

Figure  1 5 

Measured  Results  of  Bare  Transmitter/Receiver 
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Figure  16 

Bare  T ransmitter/Receiver  Beam  Pattern 
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(Eq.  5-1) 


f 

20  log 

V 


p{0) 


=  -6 


where  the  quantity  P(6)/Pax(0)  =  0-5,  the  half-amplitude.  At  half  amplitude,  0' 
was  29°  ±  1°.  A  theoretical  0'  of  27.51°  calculated  in  Chapter  IV.  Therefore  the 
-6dB  beam  width  was  2  x  0'  or  58°  which  is  5.4%  greater  than  the  theoretical 
half-amplitude  beam  width  and  16%  more  than  that  reported  in  the  technical 
data  accompanying  the  sensors.  All  comparisons  hence  forth  will  use  the  half¬ 
amplitude  as  the  threshold  of  an  acceptable  return.  The  actual  data  extracted 
during  the  experiment  is  located  in  Appendix  A. 

Next,  the  effects  of  the  transmitter  and  receiver  cones  were  examined. 
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Figure  18a 

Linear  Plot  of  Beam  Pattern  Produced  by  Small  Transmitter  Cone 
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Figure  18b 

Polar  Plot  of  Beam  Pattern  Produced  by  Small  Transmitter  Cone 
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These  side  lobes  are  undesirable  because  they  can  lead  to  false  readings. 
Appendix  A  contains  the  analog  graphs  obtained  during  this  experiment. 

The  experiment  was  then  reversed;  the  bare  transmitter  was  fixed  at  a 
distance  of  about  two  meters  and  the  receiver  and  cone  combination  was  rotated 


Experiment  3  Setup  in  Anechoic  Chamber 


As  before,  the  reduction  in  the  major  lobe  beam  width  came  at  the  expense  of 


cone.. 


the  orientation  of  the  reflecting  surface.  He  stated  that  the  sonars  had  to  be 


s 


wall  was  not  a  perfect  reflector;  however,  any  sound  energy  that  was  transmitted 
through  the  wall  material  or  absorbed  by  the  wall  material  could  be  ignored  since 
the  experiment  was  concerned  with  relative,  vice  absolute,  measurements  of 
amplitude  and  the  sound  loss  was  a  constant.  The  wall  was  placed  within  the 
far-fieid,  but  close  enough  to  the  sensor  pairs  to  allow  almost  the  entire 
transmitted  beam  to  ensonify  the  wall.  The  series  of  experiments  tested  both 
the  current  coned  sonar  configuration  and  for  the  bare  sonar  configuration.  For 
both  configurations,  the  experiments  collected  data  with  the  sonar  pair  in  both 
the  horizontal  and  vertical  positions  and  rotated  the  sonar  pair  in  both  the 
clockwise  and  counter-clockwise  directions.  Neither  the  sensor  orientation 
plane  nor  rotation  direction  affected  the  outcome.  Figures  21a  and  21  b  show 
the  experimental  setup  and  the  polar  graph  of  the  received  signal  for  the  current 
cone  configuration.  Figures  22a  and  22b  show  the  experimental  setup  and  the 
polar  graph  of  the  received  signal  for  the  bare  transducer  configuration. 


Figure  21a 

Experiment  4  Setup  in  the  Anechoic  Chamber 
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Anechoic  Chamber 


Transducer  I 

Height  off  Deck;  79.5  cm  Wall  Height:  1 24  cm 


Figure  22a 

Experiment  5  Setup  in  the  Anechoic  Chamber 
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VI.  YAMABICO-11  EXPERIMENTS 


A.  DISTANCE  MEASUREMENTS 

1.  MML10 

Since  MML  1 1  was  still  under  development  when  this  research  began, 
MML  10  and  the  Motorola  microprocessor  were  used  to  verify  the  sonar 
system's  distance  calculation  using  both  the  old,  coned  sonar  pair  configuration 
and  the  new,  bare  sonar  pair  configuration.  For  these  experiments,  a  sonar  pair 
without  cones  replaced  the  front  left  sonar  pair.  Sonar  0,  on  Yamabico-1 1 .  The 
bare  sonar  pair  mounted  its  transmitter  and  receiver  flush,  keeping  the 
separation  distance  as  before  at  45  millimeters.  The  5th  floor  of  Spanagel  Hall 
at  the  Naval  Postgraduate  School  served  as  the  experimental  laboratory.  Marks 
on  the  floor  indicated  distances  from  the  wall;  these  marks  occurred  at  10 
centimeter  increments  up  to  one  meter,  then  at  50  centimeter  increments 
thereafter  up  to  400  centimeters,  then  at  410,  415  and  420  centimeters.  These 
marks  had  an  accuracy  of  ±  0.1  centimeters.  Despite  great  care,  aligning 
Yamabico-1 1  on  the  marks  introduced  another  +0.1  centimeter  error. 

Positioning  Yamabico-1  Ts  sonar  beam  perpendicular  to  the  wall  introduced  an 
error  which  depended  on  the  distance  from  the  wall;  at  400  centimeters,  being  2 
degrees  off  of  perpendicular  introduced  a  +0.24  centimeter  error.  Using  both  the 
new  bare  sonar  pair  configuration,  Sonar  0,  and  the  old  coned  sonar  pair 
configuration,  Sonar  3,  the  experiments  recorded  the  distances  measured  by  the 
sonar  system.  Each  experimental  run  used  the  average  of  twenty-one  readings. 
Comparison  of  the  averaged  raw  range  data  points  and  the  actual  marked 
distance  produced  a  difference,  called  "Delta,"  calculated  by  subtracting  the 
distance  determined  by  the  sonar  from  the  actual  marked  distance. 

Figures  23  and  24  show  the  results  of  these  experiments  using  the 
original  circuitry.  The  slopes  of  the  best  fitting  line  to  the  data  are  within  less 
than  0.5%  of  each  other  and  the  y-intercepts  are  within  less  than  4%  of  each 
other.  There  was  less  error  in  the  distances  as  measured  by  Sonar  0;  the 
standard  error  for  Sonar  0  was  0.52  centimeters  whereas  Sonar  3  had  a 
standard  error  of  0.77  centimeters.  In  both  cases,  zero  error  occurred  at 
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Figure  23 

Error  in  Bare  Sonar  Pair  Distance  Measurement  Using  Old  Circuitry 
Equation  of  "Best  Fit"  Line:  Y  =  0.016581  X  -  3.26489 
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Figure  24 

Error  in  Coned  Sonar  Pair  Distance  Measurement  Using  Old  Circuitry 
Equation  of  "Best  Fit"  Line:  Y  =  0.016653  X  -  3.38523 


approximately  half  the  maximum  distance.  Although  Sonar  3  was  able  to  get  a 
return  at  4.053  meters,  it  failed  to  get  a  return  at  4.153  meters.  Sonar  0  failed  to 
get  a  return  at  distances  beyond  4  meters.  The  theoretical  maximum  range  in 
both  cases  was  4.214  meters.  Based  on  this  information,  it  appeared  that  the 
sensor  configurations  had  no  affect  on  the  effectiveness  of  the  current  distance 
calculation  algorithm  and  that  neither  configuration  was  able  to  achieve  the 
maximum  theoretical  range  even  under  the  ideal  circumstances  of  this 
experiment. 

Subsequently,  similar  experiments  took  data  measurements  at  50 
centimeter  increments  using  the  new  circuitry  designed  by  Michiue.  Figures  25 
and  26  show  the  results  of  these  experiments.  With  the  new  circuitry,  the  slope 
of  the  best  fitting  line  to  the  data  increased  while  the  y-intercept  remained  about 
the  same.  But  most  importantly,  with  the  new  circuitry,  both  sonars  were  able  to 
get  consistent  returns  at  over  4  meters.  These  experiments  showed  that  the 
new  circuit  design  had  increased  the  range  for  expected  returns.  However, 
these  experiments  also  demonstrated  the  inaccuracy  of  the  current  distance 
calculation  algorithm. 


Distance  Error  Bare  Sonar  Pair  (New  Circuit) 


Figure  25 

Error  in  Bare  Sonar  Pair  Distance  Measurement  Using  New  Circuitry 
Equation  of  "Best  Fit"  Line:  Y  =  0.020333  X  -  3.07433 
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To  determine  the  distance,  a  register  records  the  number  of  clock  ticks  between 


2.  MML11 


Implementation  of  the  sonar  functions  into  MML  1 1  completed 
concurrently  with  the  above  experiments  in  September  1 994.  Therefore, 
implementation  of  the  correct  clock-distance  conversion  factor  occurred  in  MML 
1 1  in  the  file  "sonar.c"  under  the  function  "SonarSysControl."  All  subsequent 
experimentation  used  MML  1 1  and  the  SPARC4  microprocessor. 

Although  it  seemed  like  a  minor  point,  dividing  the  number  of  clock  ticks 
by  ten  versus  multiplying  them  by  0.1029  greatly  affected  the  distance  error. 

The  previous  experiments  were  repeated  using  the  new  clock-distance 
conversion  factor;  Figures  27  and  28  plot  the  results.  In  both  cases,  the  slope 
went  from  positive  to  negative.  Since  "Delta"  is  the  actual  measured  distance 
minus  the  sonar  distance,  this  means  that  the  sonar  system  is  recording  a  longer 
distance  which  is  consistent  with  the  received  pulse  shape  limitations  discussed 
in  Chapter  ill. 


Distance  Error  of  Bare  Sonar  Pair  using  MML  11  and 
Clock-Distance  Conversion  Factor  of  0.1029 


Figure  27 

Bare  Sonar  Pair  Distance  Error 
(Clock-Distance  Conversion  Factor  of  0.1029) 
Equation  of  "Best  Fit"  Une:  Y  =  -  0.01214  X  -  1 .817 
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Distance  Error  of  Coned  Sonar  Pair  using  MML  1 1  and 
Clock-Distance  Conversion  Factor  of  0.1029 
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Figure  28 

Coned  Sonar  Pair  Distance  Error 
(Clock-Distance  Conversion  Factor  of  0.1029) 

Equation  of  "Best  Fit"  Line:  Y  =  - 0.0081 5X  -  2.771 

There  are  both  static  and  dynamic  causes  of  the  error.  The  static  causes 
related  to  the  clock  counter  are  minor;  the  main  cause  of  error  is  dynamic  and 
related  to  the  strength  of  the  reflected  signal  and  the  firing  of  the  Schmitt 
Trigger.  As  previously  stated,  the  strength  of  the  received  signal  is  a  function  of 
both  the  distance  to  and  the  reflectance  of  an  object. 

Additional  experiments  examined  the  amount  of  error  introduced  by  both 
distance  and  object  composition.  Adjustment  of  the  oscilloscope  to  show  the 
individual  cycles  of  the  received  signal  and  the  firing  of  the  Schmitt  Trigger  while 
the  sonar  pinged  continually  allowed  counting  of  the  number  of  cycles  received 
before  the  Schmitt  Trigger  fired.  For  the  bare  sonar  pair,  Sonar  0,  it  took  three 
cycles  at  50  centimeters  before  the  Schmitt  Trigger  fired;  at  400  centimeters,  it 
took  about  13  cycles.  For  Sonar  3,  the  coned  sonar  pair,  it  took  two  cycles  at  50 
centimeters  and  eight  cycles  at  400  centimeters.  Between  these  ranges,  the 
number  of  cycles  needed  to  fire  the  Schmitt  Trigger  rose  linearly.  Each  cycle 
caused  the  sonar  range  to  be  about  0.43  centimeters  greater  than  the  actual 
distance.  At  longer  distances,  the  number  of  cycles  needed  to  fire  the  Schmitt 
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Trigger  varied.  The  number  recorded  represented  the  average  observed  over  a 
period  of  a  couple  of  minutes. 

To  examine  the  affect  of  object  composition,  Yamabico-1 1  moved  slowly 
towards  an  object,  stopping  when  the  sonar  range  fell  below  150  centimeters. 
Recording  the  actual  distance  from  the  various  objects  to  the  front  of  the  stopped 
Yamabico-1 1  enabled  a  comparison  of  objects  with  different  material 
composition.  Although  the  value  of  actual  measurement  was  not  important,  the 
difference  between  the  measurements  was  significant.  Table  2  lists  some  of 
these  distances.  Since  the  point  at  which  the  Schmitt  Trigger  fires  varies,  it  is 
impossible  to  develop  a  software  algorithm  to  calculate  the  distance  accurately 
using  the  circuitry  of  Figure  4. 


OBJECT 

DISTANCE 

ENSONIFIED 

( ±  0.5  cm) 

Cardboard  Box 

137.0 

Carpeted  Room  Divider 

139.5 

Foam  Rubber 

130.0 

Plastic  Trashcan 

143.5 

Wall 

140.0 

Table  2 

Sonar  Distance  Variations  Based  on  Material  Composition 


B.  MULTIPATH  INTERFERENCE 


The  anechoic  chamber  experiments  showed  that  the  bare  sonar  pair 
produced  a  wide  beam  pattern  whereas  the  coned  sonar  pair  produced  a  narrow 
beam  with  side  lobes.  Therefore,  the  next  testing  configuration  placed  the  bare 
sonar  pair.  Sonar  0,  above  the  coned  sonar  pair.  Sonar  3,  at  a  height  of 
approximately  45.5  centimeters  from  the  floor  on  robot  centerline.  This  would 
allow  usage  of  both  wide  and  narrow  beam  sensors.  Additionally,  the  front 
corner  sonars.  Sonar  10  and  Sonar  1 1 ,  became  bare  sonar  pairs.  Repeating 
the  previously  described  distance  experiments  for  both  the  bare  and  coned 
sonar  pairs.  Sonar  0  and  Sonar  3  respectively,  revealed  problems.  The  coned 


sonar  pair  performed  consistently,  but  at  approximately  3  meters,  the  return 
signal  disappeared  for  bare  sonar  pair.  Although  both  the  corner  sonars.  Sonar 
1 0  and  Sonar  1 1 ,  had  the  same  bare  sonar  pair  configuration,  their  return 
signals  were  consistent.  The  only  difference  between  the  three  bare  sonar  pairs 
was  mounting  height.  The  corner  sonars  were  mounted  at  approximately  36.5 
centimeters  from  the  floor. 

Although  the  bare  sonar  pair  provided  a  significant  increase  in  obstacle 


h  =  dsm9  (Eq.  6-3) 

Combining  Equations  6-2  and  6-3  means  that  total  destmctive  interference  will 
occur  if 


9>  sin" 


2h 


n  +  - 


X  +  D 


(Eq.  6-4) 


At  a  height  of  36.5  centimeters  and  distance  of  4.214  meters,  total  destructive 
interference  will  occur  is  9.96°.  For  a  distance  of  2  meters,  a  sensor  at  this 
same  height  will  experience  total  destructive  interference  if  9>  21.35°. 
However,  the  thresholding  caused  by  the  Schmitt  Trigger  means  that  ^  is  a 
function  of  reflected  signal  strength  and  difficult  to  predict.  This  makes  it 
virtually  impossible  to  calculate  the  optimum  height  to  avoid  total  destructive 
interference  within  the  operating  range  of  Yamabico-1 1 ;  however,  one  can 
restrict  the  vertical  beam  width  to  reduce  the  reflected  amplitude. 


C.  ROTATIONAL  SCAN  ANGLE  MEASUREMENTS 


A  rotational  scan  experiment,  similar  to  that  conducted  in  the  anechoic 
chamber,  verified  the  detection  capabilities  of  both  the  bare  sonar  pair.  Sonar  0. 
and  the  coned  sonar  pair,  Sonar  3,  using  MML  1 1  with  the  SPARC4 
microprocessor.  The  rotational  scan  took  place  at  approximately  73.5,  123.5, 
173.5  and  223.5  centimeters  from  a  continuous  wall  on  the  5th  deck  of  Spanagel 
Hall  at  the  Naval  Postgraduate  School.  Fortunately,  multipath  interference  was 
not  a  problem  at  these  distances.  Instead  of  recording  raw  range  data  as  in  the 
previous  experiment,  the  experiment  recorded  the  global  x-y  coordinates  of  each 
data  point.  If  no  sonar  return  occurred  in  the  allotted  time  period,  the  raw  range 
was  set  to  infinity,  defined  for  Yamabico-1 1  as  1 .0  x  10®;  if  the  range  was 
infinity,  the  global  coordinate  did  not  print  to  the  data  file.  Figures  30  and  31 
plot  these  global  data  points. 

At  longer  distances,  the  global  coordinates  of  the  return  are  more  spread 
out  due  to  the  time  required  for  sound  to  travel  to  and  from  the  wall.  Although 
the  bare  sonar  pair  produced  smooth  data,  the  coned  sonar  pair  gave 
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inconsistent  returns  due  to  the  narrowness  of  the  beam  and  the  presence  of  side 
lobes.  The  absence  of  returns  occurred  where  the  beam  pattern  of  the  coned 
sonar  pair  had  a  node.  In  both  cases,  at  large  angles,  the  sonars  measured  a 
shorter  distance  because  the  side  of  the  beam  generated  the  return  vice  the 
center  of  the  beam.  Since  the  global  coordinate  calculation  assumed  that  the 
return  occurred  on  beam  centerline,  the  plots  in  Figures  30  and  31  curve  at  the 
edges. 

Before  this  work  began,  others  had  observed  that  Yamabico-1 1  could  not 
receive  returns  unless  almost  perpendicular  to  an  object.  At  the  time  of  these 
observations,  Yamabico-1 1  had  the  sonar  configuration  of  Figure  1  and  used  a  5 
Volt  supply  voltage.  Based  on  the  beam  patterns  produced  in  the  anechoic 
chamber,  the  original  hypothesis  made  claimed  that  the  cones  caused  this 
limitation.  However,  Figures  30  and  31  show  virtually  no  difference  between  the 
■  ingular  response  of  the  coned  and  bare  sonar  pairs  using  a  sur  “ ' 

2  Volts.  Previously,  the  amplitude  of  the  returns  generated  by 
jsing  the  5  Volt  supply  were  insufficient  to  trigger  the  Schmitt  T 
Tease  in  supply  voltage,  the  side  lobes  are  able  to  fire  the  Sch 
ing  a  similar  angular  response  as  the  bare  sonar  pair,  disprovir 
othesis. 

Rotation®!  Scan  of  Wall  by- Bare  Sonar  Pair 
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Figure  30 

Rotational  Scan  of  Wall  by  Bare  Sonar  Pair  on  Yamabico-1 1 


Rotational  Scan  of  Wall  by  Coned  Sonar  Pair 


Global  X  Coordinate  (cm) 

Figure  31 

Rotational  Scan  of  Wall  by  Coned  Sonar  Pair  on  Yamabico-1 1 


D.  OBSTACLE  AVOIDANCE 

The  Yamabico-1 1  research  group  had  demonstrated  successfully  various 
methods  for  obstacle  avoidance  using  only  ultrasonic  sensor  information.  In  the 
past,  if  the  user  programmed  Yamabico-1 1  to  move  forward  until  it  detected  an 
object,  there  were  two  basic  motions  the  user  could  use  to  avoid  the  object.  The 
user,  knowing  both  the  location  and  size  of  the  object,  could  calculate  the 
obstacle  avoidance  path  and  program  Yamabico-1 1  to  maneuver  around  the 
obstacle  using  the  pre-determined  avoidance  path.  The  user  had  to  calculate 
the  pre-determined  avoidance  path  for  each  object  encountered.  Alternatively, 
the  user  could  program  Yamabico-1 1  to  turn  right  or  left  and  perform  a  wall- 
hugging  motion  to  maneuver  around  an  object. 

Although  both  of  these  methods  have  been  demonstrated,  they  each  have 
drawbacks.  In  the  first  case,  the  user  had  to  determine  the  proper  avoidance 
path  for  each  object.  If  a  new  object  was  added,  or  the  starting  configuration 
changed,  the  avoidance  paths  had  to  be  recalculated  and  reprogrammed.  In  the 
second  case,  a  simple  wall-hugging  heuristic  in  which  Yamabico-1 1  turned  right 
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when  it  detected  an  object  could  cause  Yamabico-1 1  to  take  the  long  path 
around  an  object  as  Figure  32  demonstrates.  Combining  these  approaches  will 
give  Yamabico-1 1  the  ability  to  determine  its  own  intelligent  obstacle  avoidance 
path,  bringing  it  one  step  closer  to  being  truly  autonomous. 
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Figure  32.  Two  Obstacle  Avoidance  Paths 


In  order  to  determine  its  obstacle  avoidance  path,  Yamabico-1 1  needs 
information  about  the  size  and  location  of  the  obstacle.  With  this  information,  it 
can  decided  the  best  path  for  avoiding  the  object  intelligently  given  its  a  priori 
knowledge  of  the  world  and  its  desired  path.  Figure  33  shows  a  generic 
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obstacle  avoidance  situation  for  a  convex  polygonal  object.  This  method  uses 
an  avoidance  path  that  is  parallel  to  the  original  path  to  minimize  robot 
movements.  If  the  object  is  known,  then  the  coordinates  of  the  vertices  A  and  B 
are  known  and  intelligent  obstacle  avoidance  is  implemented  easily.  As  can  be 
seen  in  Figure  33,  the  obstacle  has  been  grown  by  a  safety  factor.  The  signed 
distances  d1  and  d2  are  calculated  by  the  formula 

d  =  -(Xj  -xjsin  d+{yi -y^)cosd+r  (Eq.  6-5) 

where  the  subscript  /  represents  Point  A  or  B,  the  subscript  c  stands  for  the 
robot's  center  position,  and  r  is  the  safety  margin.  A  positive  d  means  the  point 
is  to  the  left  of  the  robot;  a  negative  signed  distance  places  the  point  to  the 
robot's  right. 

To  calculate  the  avoidance  path,  the  magnitudes  of  d1  and  d2  are 
compared.  Ideally,  the  robot  should  maneuver  to  the  short  side  to  avoid  the 
object.  Knowing  the  appropriate  signed  distance  d,  the  avoidance  path 
configuration  is  defined  by 

(.ix,y),e,K) 


where 

x  =  x^-dsmd 
y  =  y^+dcos0 


(Eq.  6-6) 


where  the  orientation,  0,  is  that  of  the  original  path  and  curvature,  k,  is  zero, 
defining  a  straight  line. 

if  the  obstacle  detected  does  not  correspond  to  any  known  object,  then 
other  means  are  needed  to  get  the  required  information.  Although  Yamabico- 
1  Ts  current  sonar  system  can  easily  detect  the  presence  of  an  object,  it  is  not 
suitable  for  determining  the  object  size  quickly.  Using  only  its  sonar  system, 
Yamabico-1 1  would  have  to  physically  move,  testing  for  the  object,  in  order  to 
determine  the  outer  boundaries  of  the  object.  This  movement  is  time  consuming 
and  inefficient.  A  better  method  for  localizing  an  object  requires  the  use  of  a 
sensor  which  can  detect  the  width  of  an  object. 

One  solution  is  to  use  a  visual  system  which  has  the  ability  to  determine 
the  projection  of  an  object  onto  a  vertical  plane.  Given  the  range  and  the  global 
X  -  y  coordinates  of  an  object,  a  visual  system  could  determine  the  widths,  d1 
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and  d2,  in  Figure  33,  assigning  a  negative  width  to  d2.  Combining  this 
information  with  knowledge  of  the  robot's  environment  map  and  desired  path 
allows  calculation  of  a  safe  and  appropriate  path  to  avoid  the  obstacle  using 
Equation  6-6. 

Determining  when  the  robot  is  past  the  object  and  can  transition  safely 
back  to  the  original  path  is  difficult.  By  using  a  side  facing  sonar,  the  robot  can 

g  ■ 


to  the  avoidance  path  is  a  factor  of  both  the 
and  the  robot's  speed. 
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Obstacle  Avoidance  to  the  Left 


Figure  34 

Dynamic  Obstacle  Avoidance  to  the  Left 


Obstacle  Avoidance  to  the  Right 


Figure  35 

Dynamic  Obstacle  Avoidance  to  the  Right 


VII.  NEW  SONAR  DESIGN 


A.  JUSTIFICATION 

The  results  of  the  experiments  in  the  anechoic  chamber  and  on 
Yamabico-1 1  show  the  necessity  of  re-configuring  the  sonar  system  on 
Yamabico-1 1  to  provide  consistent  sonar  returns  and  to  provide  full  360° 
coverage  throughout  its  operating  range.  Simply  removing  the  cones  from  the 
sonar  pairs  will  allow  consistent  sonar  returns.  Repositioning  the  twelve  sonar 
pairs  evenly  around  the  periphery  of  the  robot  at  30°  increments  will  provide  the 
most  comprehensive  coverage.  Overlapping  the  sonar  coverage  provides  an 
added  benefit  in  that  returns  from  neighboring  sonar  pairs  can  be  compared  to 
give  a  gross  estimate  of  obstacle  orientation. 

B.  DESIGN 

1.  Hardware  System  Modifications 

The  sonar  suite  was  modified  as  follows: 

a.  Removed  cones  from  all  sonar  pairs. 

b.  Moved  Sonars  0,  2,  5  and  7  to  centerline  front,  back,  left  and 
right,  respectively. 

c.  Moved  Sonars  3, 1 ,  4,  6  to  the  right  of  Sonars  0,  2,  5  and  7, 
respectively,  at  a  30°  angle  clockwise  from  the  centerline  sonar  pair  on  each 
side. 

d.  Moved  Sonars  1 1 , 1 0,  8  and  9  to  the  left  of  Sonars  0,  2,  5  and 
7,  respectively,  at  a  30°  angle  counter-clockwise  from  the  centerline  sonar  pair 
on  each  side. 

Figure  36  shows  the  new  locations  of  the  sonars.  This  new  system 
configuration  gives  Yamabico-1 1  full  360°  sonar  coverage  with  consistent  sonar 
returns. 
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described  in  Chapter  VI  while  "avoidPathWidth(double,  POINT)"  gives  a 
dynamic  avoidance  path  based  on  the  width  of  an  object  relative  to  the  robot's 
orientation.  The  vertices  and/or  widths  are  provided  by  the  dummy  functions 
"getBoundaries(POINT)"  and  "getWidth(POINT,int)":  although  these  functions 
return  values,  the  values  must  be  inputted  manually  by  the  user.  However,  the 
structure  exists  for  future  implementation  of  these  functions. 

Additionally,  modifications  to  the  variable  naming  convention  in  the  sonar 
table  located  "sonar.c"  make  it  more  readable.  The  sonar  positional  information 
is  now  named  "sonartable[n].SonarPosit.X",  "sonartable[n].SonarPosit.Y"  and 
"sonartable[n].Theta"  where  n  stands  for  the  sonar  number.  Modifications  to  the 
files  "sonar.h"  and  "sonarmath.c"  ensured  the  consistency  of  these  changes. 


Mnemonic 

IIIISSSI 

Group 

sooo 

0 

0 

S030 

3 

1 

S060 

10 

2 

S090 

7 

0 

S120 

6 

1 

S150 

9 

2 

S180 

2 

0 

S210 

1 

1 

S240 

8 

2 

S270 

5 

0 

S300 

4 

1 

S330 

11 

2 

Table  3 

New  Sonar  Mnemonics 
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VIII.  CONCLUSIONS  AND  RECOMMENDATIONS 

A.  BEAM  WIDTH 

This  work  investigated  the  sonar  hardware  on  Yamabico-1 1 ,  including  its 
characteristics  and  limitations,  and  will  serve  as  a  major  reference  for  further 
improvements  to  the  sonar  system.  Inaeasing  supply  voltage  to  the  sonar  driver 
boards  from  5  volts  to  12  volts  caused  the  effective  beam  width  to  increase 
because  the  side  lobes  now  provided  usable  returns.  However,  the  data  was 
inconsistent  due  to  the  nodes  in  the  beam  pattern.  Removing  the  cones 
eliminated  the  side  lobes,  providing  consistent  range  data.  The  effective  beam 
width  without  the  cones  is  about  the  same  as  it  was  with  the  cones.  The  wider 
beam  width  produced  by  the  increased  supply  voltage  has  the  negative  effect  of 
introducing  multipath  interference.  The  wider  beam  width  improves  the  sonar 
system  coverage,  but  signal  cancellation  reduces  the  range  of  the  sonar  system. 
The  range  can  be  increased  by  increasing  the  height  of  the  sensors,  but  this 
also  increases  the  minimum  detection  distance  of  objects  near  the  floor.  Using  a 
height  of  about  36.5  centimeters  for  the  sensors,  consistent  range  data  is 
achievable  out  to  a  range  of  over  2  meters.  Over  this  range,  the  data  return  is 
intermittent  due  to  the  cancellation  affects  of  multipath  interference.  An  even 
re-distribution  of  the  twelve  sonar  pairs  around  the  periphery  ensures  that 
Yamabico-1 1  has  360®  sonar  coverage.  With  twelve  evenly  spaced  sonars,  full 
coverage  can  be  achieved  with  a  minimum  full  beam  width  of  30®.  A  smaller 
beam  width  will  reduce  the  multipath  interference  effects.  This  smaller  beam 
width  can  be  achieved  by  placing  the  sensors  in  a  properly  shaped  horn. 
Investigation  of  the  proper  shape  for  the  horn  is  left  for  future  work. 

B.  SIGNAL  PROCESSING 

The  current  signal  processing,  which  uses  a  threshold  to  determine 
detection  of  a  return  signal,  limits  the  ability  of  Yamabico-1 1  to  calculate  sonar 
ranges  accurately .  The  current  threshold,  set  at  1 .4  Volts,  may  or  may  not  be 
reached  by  the  time  the  return  signal  hits  its  maximum  at  0.5  milliseconds  after 
the  return  signal  first  reaches  the  receiver.  Therefore,  thresholding  causes  a 
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dynamic  error  of  up  to  ±  8.6  centimeters.  To  remove  dynamic  error,  the  signal 
processing  must  change.  Since  the  occurrence  of  the  return  signal's  peak  is 
predictable  and  constant,  it  could  be  used  to  stop  the  clock  counter,  thereby 


APPENDIX  A. 

DATA  FROM  ANECHOIC  CHAMBER  EXPERIMENTS 


This  appendix  contains  the  analog  graphs  recorded  by  the  HP  7090A 
Plotter  during  the  anechoic  chamber  experiments  discussed.  In  each  case,  the 
rotator  speed  was  1  rpm.  The  direction  of  rotation  varied  for  each  experiment. 
Each  graph  has  the  x-axis  divided  into  10°  increments.  The  peak  voltage  occurs 
at  0°.  This  series  of  experiments  compares  the  beam  widths  determined  by  the 
x-axis.  The  y-axis  scale  varies  among  the  graphs,  but  no  comparison  of  voltage 
is  made  among  the  graphs.  The  voltage  is  only  a  factor  in  determining  the  half¬ 
amplitude  point. 

Graph  A  corresponds  to  Experiment  1  in  Chapter  V.  This  data  was 
collected  on  April  24, 1994.  The  bare  Nicera  transmitter  was  rotated  while  the 
bare  receiver  was  held  steady  at  a  distance  of  about  199.3  centimeters. 

Graph  B  corresponds  to  Experiment  2  in  Chapter  V.  This  data  was 
collected  on  April  24, 1 994.  The  transmitter  with  small  cone  was  rotated  while  a 
bare  receiver  was  held  steady  at  a  distance  of  about  198.0  centimeters. 

Graph  C  corresponds  to  Experiment  3  in  Chapter  V.  This  data  was 
collected  on  May  20, 1994.  The  receiver  with  large  cone  was  rotated  while  a 
bare  transmitter  was  held  steady  at  a  distance  of  about  200.0  centimeters. 

Graphs  D,  E  and  F  support  to  Experiment  4  in  Chapter  V.  The  data  was 
collected  by  rotating  a  coned  sonar  pair  with  a  wall  located  at  a  distance  of 
about  76  centimeters.  For  Graph  D,  produced  on  April  7, 1 994,  the  coned  sonar 
pair  was  mounted  horizontally  with  the  transmitter  t  the  left  of  the  receiver. 

Graph  E  and  F,  produced  on  April  13, 1994,  mounted  the  coned  sonar  pair 
vertically.  In  Graph  E,  the  receiver  was  above  the  transmitter  and  in  Graph  F, 
the  receiver  and  transmitter  were  reversed.  The  beam  pattern  produced  in  all 
three  graphs  was  the  same. 

Graphs  G,  H,  and  I  support  Experiment  5  in  Chapter  V  and  were  produced 
on  April  1 3, 1 994.  In  this  series,  a  bare  sonar  pair  was  rotated  about  80 
centimeters  from  the  wall.  In  Graph  G,  the  bare  sonar  pair  was  mounted 
vertically  with  the  receiver  above  the  transmitter.  In  Graphs  H  and  I,  the  bare 
sonar  pair  was  mounted  horizontally  with  the  transmitter  to  the  left  of  the 
receiver.  In  Graph  H,  the  bare  sonar  pair  was  rotated  counter-clockwise  and 
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APPENDIX  B.  USER  PROGRAMS 


This  appendix  contains  the  user  programs  written  to  perform  testing  on 
Yamabico-1 1 .  Each  file  contains  an  explanation  in  the  heading  and  indicates 
the  MML  version.  User  files  written  in  MML  10  and  MML  1 1  differ  greatly. 
Comments  within  each  user  file  explain  the  logic. 
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*  User  File:  DistTest 

^  Description:  Tests  distance  accuracy  of  sonar 

*  MML  Version:  MML  10 

*  Author:  Jane  Lochner 

*  Date:  15  JUL  94 

*  Notes:  Change  #define  statement  to  reflect 

*  sonar  number  or  mnemonic  of  sonar 

*  under  investigation.  Once  program 

*  is  run,  be  sure  to  rename  the  data 

*  file  '‘RAW"  on  the  host  before 

*  running  the  program  again. 


#include  "mml.h“ 

#define  SONAR  3 
#define  FILETYPE  0 
#define  FILENUMBER  0 

User ( ) 

{ 

long  int  i ; 
void  initialize  {)  ; 
void  cleanupO; 

initialize ( )  ; 

motor_on=OFF ; 

do 

{ i + + ; 

sonar (SONAR) ; }  /*  Gives  21  readings  */ 

while  (i<50000); 

cleanupO; 


void  initialize 0  /*  sets  up  data  logging  */ 

{ 

enable__sonar  { SONAR)  ; 
set_log_interval  (SONAR,  1)  ; 

enable_data_logging (SONAR, FILETYPE, FILENUMBER) ; 

} 

void  cleanupO  /*  transfers  data  to  host  */ 

{ 

disable_sonar (SONAR) ; 

disable_data_logging (SONAR, FILETYPE) ; 
xfer_raw_to_host (FILENUMBER, "RAW") ; 

} 
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*  User  Pile:  ContSonar 

*  Description:  Continuously  pings  given  sonar 

*  MML  Version:  MML  11 

*  Author:  Jane  Lochner 

*  Date:  6  OCT  94 

*  Notes:  Change  #define  statement  to  reflect 

*  sonar  number  or  mnemonic  of  sonar 

*  under  investigation-  Program  must 

*  be  stopped  using  the  manual  interrupt 

*  switch  on  Yamabico-11. 


/ 


#include  "user.h" 

#define  SONARUSED  0 


void 
user ( ) 

{ 

EnableSonar( SONARUSED) ; 
do 

{ 

Sonar (SONARUSED) ; 

) 

while  (TRUE) ; 
DisableSonar (SONARUSED) ; 
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*  User  File:  Scan 

*  Description:  Tests  sonar  scanning  function 

*  MML  Version:  MML  11 

*  Author:  Jane  Lochner 

*  Date:  15  OCT  94 

*  Notes:  Prints  out  to  the  screen  the 

*  Global  X-Y  Coordinates  of  object 

*  detected. 


#include  "user.h 


void 
user  ( ) 

( 

POINT  obstacle; 

obstacle  =  ScanSonar (FORWARD, 150 . 0 ) ; 

printfC'X  coord  is:  %f\nY  coord  is:  %f\n\n*' , obstacle .X, obstacle ,Y) 


/ 

★ 

*  User  File:  RotScanWall 

*  Description:  Records  the  Global  X-Y  Coordinates 

*  of  objects  detected. 

*  MML  Version:  MML  11 

*  Author:  Jane  Lochner 

*  Date:  7  OCT  94 

*  Notes:  Change  #define  statement  to  reflect 

*  sonar  number  or  mnemonic  of  sonar 

*  under  investigation.  Used  to  determine 

*  the  angular  response  of  a  given  sonar 

*  by  facing  the  sonar  down  the  hallway, 

*  then  rotating  180  degrees.  Infinite 

*  returns  can  be  deleted  from  the  data 

*  file  and  results  plotted. 

#include  "user .h“ 


#define  SONARUSED  3 
#define  USERBUFSIZE  0 
#define  FREQ  1 
#define  MODE  SONAR^GLOBAL 

void 
user  ( ) 

{ 

CONFIGURATION  Start , Current ; 
POINT  obstacle; 


EnableSonar ( SONARUSED) ; 

SonarLog (FREQ, USERBUFSIZE, SONARUSED, MODE) ; 


Start  =  def ineConf ig (0 . 0, 0 . 0 , 0 . 0 , 0 . 0) ; 
setRobotConf igImm(Start ) ; 
setRotVelImm( 5 . 0 ) ; 

Current  =  getRobotConfigO; 

Rotate (PI);  . 

waitSec (10) ; 

) 
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*  User  File:  SonarTest 

*  Description:  Gives  distance  from  sonar  face  to 

*  object  in  centimeters 


*  MML  Version:  MML  11 

*  Author:  Jane  Lochner 

*  Date:  15  SEP  94 

*  Notes:  Change  #define  statement  to  reflect 

*  sonar  number  or  mnemonic  of  sonar 

*  under  investigation. 


#include  "user.h" 


#define  SONARUSED  3 
#define  USERBUFSIZE  0 
#define  FREQ  5 
#define  MODE  SONAR_RAW 

void 
user  ( ) 

{  . 


long  int  i ; 

EnableSonar (SONARUSED) ; 

SonarLog (FREQ, USERBUFSIZE, SONARUSED, MODE) ; 


do 

{ 

LogSonar Data (SONARUSED) ; 

i++;}  /*  Gives  7  distance  readings  */ 

while  (i<50); 

DisableSonar (SONARUSED) ; 
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*  User  File:  demo 

*  Description:  Robot  moves  until  1  meter  from 

*  an  object  then  executes  a  135  degree  turn. 

*  Robot  will  repeat  this  maneuver  until 

*  the  manual  interrupt  button  is  pushed. 

*  MML  Version:  MML  11 

*  Author:  Jane  Lochner 

*  Date:  29  SEP  94 

*  Notes:  Change  #define  statement  to  reflect 

*  sonar  number  or  mnemonic  of  sonar 

*  under  investigation. 


#include  "user.h" 

#define  SONARUSED  0 


void 
user  ( ) 
{ 


int  GOING  =  1; 
double  hit; 

CONFIGURATION  Start,  JustGo,  CurrentPosit ,  Jump,  NewPosit; 

Start  =  def ineConf ig(0 . 0,  0.0,  0.0,  0.0); 

JustGo  =  defineConf ig(0 . 0,  0.1,  0.0,  0.0); 

Jump  =  defineConf ig(0 .0,  45 . 0 , -1 . 5*HPI ,  0.0); 


EnableSonar (SONARUSED) ; 
setLinVelImm(15.0) ; 
setRobotConfigImm( Start) ; 
hit  =  9999.9; 
line (JustGo) ; 
do{ 

while(hit  >=100,0  II  hit  <=  1.0) 

{ 

hit  =  Sonar (SONARUSED) ; 
printf(“\n  Range  is:  %f  ",hit); 

} 

CurrentPosit  =  getRobotConf ig ( ) ; 

NewPosit  =  compose  (ScCurrentPosit,  ScJump)  ; 

setRobotConfigImm (NewPosit ) ; 

hit  =  9999.9; 

printf ( ” \n\nNew  Sonar  Range  is  %f\n",hit); 
waitSec (2) ; 

}while (GOING) ; 


} 
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/**★*★**★★*★*★****★★*★★★★★★★★★*★*★★*★★***★******★*★*** 

* 

*  User  File:  avoidPathVertex 

*  Description:  Tests  ability  of  Yamabico  to  determine 

*  obstacle  avoidance  path  autonomously. 

*  MML  Version:  MML  11 

*  Author:  Jane  Lochner 

*  Date:  26  NOV  94 

*  Notes:  Change  #define  statements  to  reflect 

*  sonar  number  or  mnemonic  of  sonar 

*  under  investigation,  safety  factor,  and 

*  obstacle  notification  distance 

#include  "user.h" 

#define  SONARUSED  0 
#define  USERBUFSIZE  0 
#define  FREQ  10 
#define  MODE  SONAR^RAW 
#define  SAFETY  20.0 
#define  DISTANCE  150.0 
#define  DIRECTION  FORWARD 

void 
user  ( ) 

( 

double  width, hit , distance; 

CONFIGURATION  Start , JustGo, Avoid, vehicle ; 

POINT  obstacle; 
int  sonar; 

Start  =  defineConfig(0.0, 0.1,  PI/6, 0 .0)  ; 

JustGo  =  def ineConfig (0 . 0 , 0 . 0 , PI/6,  0 . 0) ; 

MotionLog (NULL, 25, 0) ; 

EnableSonar (SONARUSED) ; 

setRobotConfig (Start ) ; 
setLinVelImm(  15 . 0 )  ; 

line (JustGo) ; 

obstacle  =  ScanSonar (DIRECTION, DISTANCE) ; 
vehicle  =  getRobotConf ig ( ) ; 

Avoid  =  avoidPathVertex (SAFETY, obstacle) ; 

width  =  -  (Avoid .  Posit  .X- vehicle .  Posit . X)  *sin  (vehicle . Theta)  -i- 
(Avoid. Posit .Y-vehicle . Posit .Y) *cos (vehicle .Theta) 


if  (width  <  0) 

{ 

width  =  width  -  SAFETY; 
sonar  =  S270; 
distance  =  -width; 

} 

else 

{ 

width  =  width  +  SAFETY; 
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sonar  =  S090; 
distance  =  width; 

} 

line (Avoid) ; 
while 

{ (vehicle . Posit .X  < (Avoid . Posit .X+DISTANCE*cos (JustGo .Theta) ) ) 

I  I 

( vehicle . Posit .Y  <  (Avoid . Posit .Y+DISTANCE*sin (JustGo .Theta) )) ) 

{ 

vehicle  =  getRobotConf ig ( ) ; 
waitMS (500) ; 

) 

EnableSonar (sonar) ; 
hit  =  Sonar (sonar) ; 
waitMS (30 )  ; 

while (hit  <=  distance) 

{ 

hit  =  Sonar (sonar) ; 
printf ( "Distance  =  %f\n",hit); 
waitMS (30) ; 

} 

line (JustGo) ; 
waitSec (20 )  ; 
stoplmm ( ) ; 

) 
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APPENDIX  C.  MML11  LIBRARY  FILES 


This  appendix  contains  the  MML1 1  files  which  contain  changes 
resulting  from  this  work.  The  heading  of  each  new  function  explains  the  inputs 
and  outputs  of  the  functions  and  describes  the  function’s  use. 
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*  File:  sonar. h 

*  Comments:  12-07-94  Updated  for  new  sonar  naming 

*  convention  by  Jane  Lochner. 

***★★★★★★★★***★**★★*★*★***★*★★*★*★★★★•*■★★*★★★★**★****★/ 
#ifndef  _ SONAR_H 

#define  _ SONAR_H 


#include  " constants .h“ 
#include  "definitions .h" 

#define  NUM^SONARS  16 


/*  Sonar  locations  */ 


#def ine 

SOOO 

0 

#def ine 

S030 

3 

#def ine 

S330 

11 

#def ine 

S090 

7 

#def ine 

S060 

10 

#def ine 

S120 

6 

#def ine 

S180 

2 

#def ine 

S150 

9 

#def ine 

S210 

1 

#def ine 

S270 

5 

#def ine 

S240 

8 

#def ine 

S300 

4 

/*  Types  of  sonar  logging  */ 


#def ine 

SONAR^NONE 

0x00 

#def ine 

SONAR^RAW 

0x01 

#def ine 

SONAR^GLOBAL 

0x02 

#def ine 

SONAR_SEGMENT 

0x04 

#def ine 

S0NAR_ALL 

0x07 

#define  SONAR_CTL  0xfc0083f9 


typedef  struct  { 

int  fitting,  /*flag  to  indicate  linear  fitting  request  */ 

globalCoord,  /*flag  to  indicate  coordinate  conversion  request  */ 

update;  /*flag  to  indicate  presence  of  new  data  */ 


double 


d,  /* 
t,  /* 
SonarTheta; 


range  data  */ 

robot ' s.  orientation  angle  at  time  of  range  */ 
/*  angle  of  sonar  from  robot  center  */ 


POINT  posit;  /*  robot's  position  at  time  of  range  (x,  y)  */ 

POINT  global;  /*  global  position  of  sonar  return  (gx,  gy)  */ 

POINT  SonarPosit;  /*  position  of  sonar  from  center  (rob_x,  rob_y)  */ 
}  SONARD; 


/*  defines  a  basic  segment  with  the  start  and  end  points,  and  the  sonar 
it  is  associated  with  */ 

typedef  struct  { 

POINT  start; 

POINT  end; 

double  alpha,  /*  angle  and  length  of  normal  from  origin  */ 

r;  /*  to  the  segment  */ 

int  sonarNumber ; 

}  SEGMENT; 
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typedef  struct  { 

/*  (headx,  heady,  tailx,  tally,  sonar,  alpha,  r)  */ 
SEGMENT  seg; 

/*  length  of  the  segment  */ 

double  length; 

)  LINE^SEG; 


typedef  struct  {  /*  revised  by  Y.  Kanayama,  07-07-93  */ 

double  mOO,  /*  moments  */ 

mlO , 
mOl , 
m20 , 
mil , 
m02  ; 

SEGMENT  seg;  /*  (startx,  starty,  endx,  endy,  n,  alpha,  r)  */ 
)  CUR^DATA; 


/***  Global  variables  ***/ 
extern  int  service_f lag; 

extern  SONARD  sonar_table [ ] ; 


/***  Prototypes  ***/ 

void  InitSonar (void) ; 

double  WaitSonar { int ) ; 


/*  Interrupt  handler  */ 
void  SonarSysControl (void) ; 

/*  So  the  user  doesn't  have  to  include  all  the 
sonar  header  files...  */ 

# include  “sonarcard.h" 

# include  " sonarmath .h" 

#  include  sonar  log ,  h* 

#endif 
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/*♦**★★**★★•*■★★★★★*★*★★★★*★★★*★★★★*★★★*******★*★★*****♦*****■*■ 

*  File:  sonarmath.h 

*  Coiranents:  12-07-94  Update  to  support  scanning  function 

*  and  new  functions  related  to 

*  dynamic  obstacle  avoidance. 

*  Update  for  sonar  table  changes  of 

*  rob_t  to  SonarTheta  and 

*  offset  to  SonarPosit 
★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★***************************** 

#ifndef  _ SONARMATH^H 

#define  _ SONARMATH^H 


--  Added  by  Jane  Lochner  */ 


# include  " sonar . h " 

/*  Types  of  Scanning 
#define  FORWARD 
#define  BACKWARD 
#define  LEFT 
#define  RIGHT 


/*  The  following  typedef  was  added  to  support  the 
getBoundaries ( )  function  written  as  part  of  the 
thesis  work  of  LCDR  Lochner.  */ 

typedef  struct  { 

POINT  left; 

POINT  right; 

}  BOUNDARY; 

void  InitSonarmath(void)  ; 

void  SetSonarParameters (double,  double) ; 

double  Sonar { int ) ; 

POINT  Global (int); 

LINE_SEG  *GetSegment (int) ; 

LINE_SEG  ^EndSegment ( int) ; 

void  CalculateGlobal (int) ; 

void  Generat eSegment ( int ) ; 

void  EnableLinearFitting(int) ; 

void  '  DisableLinearFitting(int) ; 
void  LinearFitting(int) ; 


/*  The  following  prototypes  were  added  to  support 
the  new  functions  added  in  sonar. c  as  part  of 
the  thesis  work  of  LCDR  Lochner  */ 

POINT  ScanSonar( int, double) ; 

BOUNDARY  getBoundaries ( POINT) ; 

double  getWidth ( POINT, int ) ; 

CONFIGURATION  avoidPathVertex (double, POINT) ; 
CONFIGURATION  avoidPathWidth (double , POINT) ; 


#endif 
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*  Author  ;  Patrick  Byrne  ,  Yutaka  Kanayama 

*  Date  :  20  November  1993 

*  File  ;  sonar, c 

*  Description  :  Provides  the  global  generic  sonar  functions 


*  Comments : 

*  -  Fri  07-22-94  Updated  for  Sparc  mmlll  FEK 

*  -  updated  by  Khaled  morsy  11-22-94 

*  -  Updated  on  6  Dec  94  by  Jane  Lochner  to  add  new 

*  sonar  positions  and  to  ren^une  offset  to  SonarPosit 

*  and  rob_t  to  SonarTheta  in  the  sonar  table. 


# include 
#include 
#include 
# include 
# include 
# include 
# include 
# include 
# include 


"definitions .h" 
"memsys  .h* 
"motion. h“ 
"sonarcard.h" 
"sonarmath.h" 
"sonar log. h" 

" system. h" 
"time.h" 

"sonar .h" 


/★★★  Global  variables  ***/ 
int  service_flag  ; 

SONARD  sonar_table [NUM_SONARS] ;  /*one  of  the  above  struct 's  for  each  sonar  */ 


/*  used  by  ServeSonar  */ 

static  const  int  group_array [ 4 ] [4]  =  { 

{0,  5,  2,  7}, 

{3,  4,  1,  S), 

{10,  11,  8,  9}, 

(12,  13,  14,  15) 

};  /*  array  maps  sonar  niambers  to  groups  ♦/ 


void 

InitSonar (void) 

{ 

int  i ; 

/*  .initialize  sonar_table  */ 
for  (i  =  0;  i  <  NUM^SONARS;  i++) 

memset (&sonar_table[i] ,  0,  sizeof (SONARD) ) ; 

/*  set  up  compensation  for  sonar  position  */ 


sonar_table [ 0 ]. SonarTheta  =  0.0;  /**/ 

sonar_table [ 1 1 . SonarTheta  *  5.0*PI/6.0;  /**/ 

sonar_table [2 J .SonarTheta  =  PI;  /**/ 

sonar_table[3] .SonarTheta  =  -PI/6.0;  /**/ 

sonar_table[ 4] .SonarTheta  =  PI/3.0;  /**/ 

sonar_table( 5] .SonarTheta  =  PI/2.0;  /**/ 

sonar_table[ 6] .SonarTheta  ^  -2.0*PI/3.0;  /**/ 

sonar_table [7] .SonarTheta  =  -PI/2.0;  /*^/ 

sonar_table [8] .SonarTheta  =  2.0*PI/3.0;  /**/ 

sonar_table [9] .SonarTheta  =  -5.0*PI/6.0;  /**/ 

sonar_table[ 10] .SonarTheta  =  -PI/3.0;  /**/ 

sonar_table[ 11] .SonarTheta  «  PI/6.0;  /**/ 

sonar_table[ 12] .SonarTheta  =  0.0; 
sonar_table[ 13] .SonarTheta  =  1.5708; 
sonar_table[ 14] .SonarTheta  =  4.7124; 
sonar_table[ 15] .SonarTheta  =  0.0; 
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sonar_table [0] .SonarPosit ,X  =  23.6;  /**/ 
sonar_table [1] .SonarPosit .X  =  -23.0;  /**/ 
sonar_table [2 ] . SonarPosit .X  =  -22.6;  /**/ 
sonar_table [3 ] .SonarPosit .X  =  24.7;  /**/ 
sonar_table [4] .SonarPosit .X  =  13.4;  /**/ 
sonar_table [ 5 ] , SonarPosit .X  =  0.0;  /**/ 
sonar^table [6] .SonarPosit .X  =  -12.6;  /**/ 
sonar_table [7 ] .SonarPosit .X  =  0.0;  /**/ 
sonar_table [8 ]. SonarPosit .X  =  -13.4;  /**/ 
sonar_table [9] .SonarPosit .X  =  -23.5;  /**/ 
sonar_table [10] .SonarPosit .X  =  12.1;  /**/ 
sonar_table [ 11 ]. SonarPosit .X  =  25.2;  /**/ 
sonar_table [ 12 ]. SonarPosit .X  =  0.0; 


sonar_table [ 13 ]. SonarPosit .X  =  1.5708; 
sonar_table [14] .SonarPosit .X  =  4.7124; 
sonar_table [ 15 ]. SonarPosit .X  -  0.0; 


sonar_table[0] .SonarPosit .Y  =  -0.5;  /**/ 

sonar_table[l] .SonarPosit. Y  =  13.1;  /**/ 

sonar_table [2] .SonarPosit .Y  =  -1.0;  /**/ 

sonar_table [3 ]. SonarPosit .Y  =  -14.6;  /**/ 

sonar_table [4] .SonarPosit .Y  =  21.3;  /**/ 

sQnar_table [5] .SonarPosit .Y  =  20.6;  /**/ 

sonar_table [6] .SonarPosit .Y  =  -21.3;  /**/ 

sonar^table [7] .SonarPosit. Y  =  -20.5;  1^*1 

sonar_table  [8]  .SonarPosit  .Y  =  21.3;  /*■*"/ 

sonar_table [9 ]. SonarPosit .Y  =  -14.9;  /★*/ 

sonar__table  [10]  .Sonar  Posit.  Y  ^  -21.3;  /**/ 

sonar_table [11] .SonarPosit .Y  =  14.1;  /**/ 

sonar_table [12] .Sonar Posit. Y  =0.0; 
sonar_table  [13  ]  .SonarPosit  .Y  =  21.5;. 
sonar_table [14] .SonarPosit .Y  =  21.5; 
sonar_table [ 15 ]. SonarPosit .Y  =  0.0; 


/*  initialize  the  sonar  components  */ 
InitSonarmath( ) ; 

InitSonarlog ( ) ; 

SetSonarParameters (0.02,  5.0); 


/*****★*★★***★*★***★★*★**•*★★★★*★★★★★★★★★★★**★★★★★★★★★★**★*★★★*★★*★★★★★ 

*  Procedure:  wait_sonar (n) 

*  Description:  waits  in  a  loop  until  new  data  is  available  for 

*  sonar  n . 


double 

WaitSonar ( int  n) 

{ 

sonar_table [n] .update  =  0; 
while  { sonar_t able [n] .update  ==  0) 
;  /*  NULL  statement  */ 

return  sonar_table[n] .d; 

} 
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*  Procedure :  serve_sonar ( x, y , t , ovf 1 , datal , data2 , data3 , data4 , group) 

*  Description:  this  procedure  is  the  "central  command"  for  the 

*  control  of  all  sonar  related  functions.  It  is  linked  with  the 

*  ih_sonar  routine  and  loads  sonar  data  to  the  sonar_table  from 

*  there.  It  then  exeunines  the  various  control  flags  in  the 

*  sonar_table  to  determine  which  activities  the  user  wishes  to  take 

*  place,  and  calls  the  appropriate  functions.  This  procedure  is 

*  invoked  approximately  every  thirty  milliseconds  by  an  interrupt 

*  from  the  sonar  control  board. 

void 


SonarSysControl (void) 

{ 


static  int 

int 

int 

int 

int 

CONFIGURATION 


cnt  =  0 ; 

n; 

i; 

data [ 4 ] ; 
group ; 
current ; 


/*  overflow  bit  is  bit 
#define  OVERFLOWMASK 
#define  GROUP_MASK 
#define  SONAR_DATAO 
#define  SONAR^DATAl 
#define  S0NAR_DATA2 
#define  S0NAR„DATA3 


15  */ 

0x8000 

0x18 

0xfc0083f0 

0xfc0083f2 

0xfc0083f4 

0xfc0083f6 


/*  blink  the  #1  LED  */ 
if  (++cnt  >10)  { 

cnt  =  0; 

changeLEDstate ( 1 ) ; } 
current  =  getRobotConfigO ; 


group  =  ( {*(BYTE*)SONAIUCTL  &  GROUP JMASK)  »  3); 

data[0]  =  *(WORD*)SONAR_DATAO; 

data[l]  =  *(WORD*)SONAR_DATAl; 

data [2]  =  ^ (WORD*)SONAR_DATA2; 

data  13]  =  * (WORD*) SONAR_DATA3 ; 


for  (i  =  0;  i  <  4;  i+-i-)  { 

n  =  group_array [group] [i] ; 
sonar_table[n] .posit .X  =  current .Posit. X; 
sonar_table[n] .posit .Y  =  current.Posit.Y; 
sonar_table(n] . t  =  current .Theta; 

/*  -1  was  returned  if  there  was  no  echo  */ 

if  (datati]  &  OVERFLOWMASK) 

sonar_table[nl  .d  s  INFINITY; 

else 

{  /*  only  first  12  bits  are  data,  so  mask  the  data  */ 

data[i]  &=  Oxfff; 

sonar_table[n] .d  =  data[i]  *  0.1029;  } 

CalculateGlobal (n) ; 
if  (sonar_table[n] .fitting  ==  1) 

LinearFitting(n) ; 

/*  log  the  data  for  this  sonar  */ 

LogSonarData(n) ; 

} 


} 
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/ 

*  Author  :  Patrick  Byrne 

*  Date  :  20  November  1993 

*  File  :  sonarmath.c 

*  Description  :  Provides  the  main  sonar  functions 

* 

*  Comments; 

*  Fri  07-22-94  Updated  for  Sparc  mmlll  FEK 

*  Wed  12-07-94  Five  new  functions  added  by 

*  Jane  Lochner  to  support  scanning 

*  and  dynamic  obstacle  avoidance  ' 

#include  "def init ions . h" 

# include  “ sonar. h" 

#include  "stdiosys .h" 

# include  " math . h " 

# include  “ mot ion. h" 

# include  "memsys.h" 

# include  " sonar log .h” 

# include  " sonarmat h . h “ 

#define  QMAX  50 
#define  SONARS  11 

/***  Local  variables  ***/ 
static  double  Cl,  C2; 

static  LINE^SEG  Queue [SONARS] [QMAX] ; 
static  int  Head [SONARS] ; 

static  int  Tail [SONARS] ; 

static  int  Empty [ SONARS ] ; 

static  LINE_SEG  segstruct;  /*  temporary  storage  for  get_segment  func.  */ 

static  int  SegListHead [NUM_SONARS] ;  /*points  to  oldest  segment  array  element  */ 

static  int  SegListTail [NUM_SONARS] ;  /^points  to  newest  segment  array  element  */ 

static  LINE_SEG  seg_list [NUM_SONARS] [5 ] ;  /*segments  for  working  memory 

static  CUR_DATA  segment_data [NUM_SONARS] ;  /^interim  data  for  all  sixteen  sonars  */ 

/***  Global  variables  ***/ 

/***  Local  Prototypes  ***/ 
void  Enqueue  (int,  LINE^SEG*^)  ; 

void  AddToSegment ( int ,  POINT); 

void  ResetMoments ( int ) ; 

void  BuildList (LINE_SEG*,  int); 


Code  ***/ 

void 

InitSonarmath (void) 

{ 

int  i  ; 

for  (i  =  0;  i  <  NUM^SONARS;  i++)  { 

ResetMoments ( i) ; 

memset (&segment_data [ i] ,  0,  sizeof (CUR_DATA) ) ; 


} 


} 


Empty [i]  =  TRUE; 
Head[i]  =  1; 
Tail[i]  =  1; 
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*  Procedure :  set_sonar_paraineters  ( cl ,  c2) 

*  Description:  allows  the  user  to 

*  adjust  constants  which  control  the  linear  fitting  algorithm.  Cl  is 

*  a  multiplier  to  allow  more  lenancy  for  greater  sonar  ranges. 

*  C2  is  an  absolute  value;  both  are  used  to  determine  if  an 

*  individual  data  point  is  usable  for  the  algorithm.  Default  values 

*  are  set  in  main.c  to  .02,  5.0  respectively. 

void 

SetSonarPareimeters  (double  cl,  double  c2) 

{ 

Cl  =  cl; 

C2  =  c2 ; 


^**ie*irifkieit*ir*irifk***ir*it*-kit***’kit**ie*-kie**if*‘kif'k******-k**-k*-k*****ifkieit*-kieifkit 

*  Procedure:  sonar (n) 

*  Description:  returns  the  distance  (in 

*  centimeters)  sensed  by  the  n_th  ultrasonic  sensor.  If  no  echo  is 

*  received,  then  INFINITY ( 1 . 0e6)  is  returned.  If  the  distance  is  less  tfian  10 

*  cm,  then  a  0  is  returned. 

double 
Sonar (int  n) 

{ 

return  sonar^tablefn] .d; 

} 


*  Procedure:  global (n) 

*  Description:  returns  a  structure  of  type 

*  posit  containing  the  global  x  and  y  coordinates  of  the  position  of 

*  the  last  sonar  return. 

POINT 

Global (int  n) 

{ 

return  sonar_table [n] .global; 

} 

void  Enqueue  0 

This  function  is  called  by  gene rat e_s egment () .  The 
sonar  n\amber  and  newest  line_segment  for  that  sonar 
are  passed  int.  It  simply  places  the  latest  segment 
produce  by  a  sonar  with  linear^f itting  and  places  it 
into  a  circular  queue. 

■k  it  ********  it  it  it  •kit’kir  if  ******•/(***  it  *  it-k  ^ 

void 

Enqueue (int  i,  LINE_SEG  ♦Seg) 

{ 

int  j  ; 

if  (Head[i]  =:=Tail[i]  Se&  Empty [i]  ==  FALSE) 
printf ( "Sonar  segment  queue  is  Full"); 
else  { 

j  =  Tail[i] ; 

Queue[i][j]  =  *Seg; 

Tail[i]  =  1  +  (Tail[i]  %  QMAX) ; 

Empty [i]  =  FALSE;  93 

) 

} 


/*★*****★★★★*★******★*★*★★★★★*★★★★★★★*★*★*★*★★***★** 
LINE_SEG  get_seginent  (sonar) 

returns  the  pointer  to  the  oldest  completed  unread 
segment  of  the  sonar  passed  in.  If  there  is  no  completed 
unread  segment  NULL  is  returned. 

LINE^SEG  * 

GetSegment ( int  i) 

( 

L INE_SEG  *Cur rentes  eg ; 

int  j  ; 

if  (Empty[i]) 

Current_seg  =  NULL; 
else  { 

j  =  Head[i] ; 

Current_seg  =  &Queue [ i] [ j ] ; 

Head[i]  =  1  +  (Head[i]  %  QMAX) ; 
if  (Head[i]  ==  Tail[i]) 

Empty  [i]  =  TRUE; 

}• 

return  Current_seg; 

} 


/**♦★★★*★★*★★★*★★★★****★**★★★★★★*★★★★★★*****★★★★***★★★★*★★★★★**★★★********★★** 

*  Procedure:  EndSegment (n) 

*  Description:  this  procedure  allocates 

*  memory  for  the  segment  data  structure,  loads  the  correct  values 

*  into  it  and  returns  a  pointer  to  the  structure. 


LINE^SEG  * 

EndSegment ( int  n) 

{ 

SEGMENT  tmpSeg; 

LINE_SEG  *seg_ptr; 

double  length,  delta; 

seg_ptr  =  &segstruct; 

tmpSeg  =  segment_data [n] .seg; 

delta  =  tmpSeg . start .X  *  cos (tmpSeg. alpha)  + 

tmpSeg. start .Y  *  sin (tmpSeg. alpha)  -  tmpSeg.r; 
tmpSeg. s tart .X  -=  delta  *  cos(tmpSeg.alpha); 
tmpSeg. start .Y  -=  delta  *  sin (tmpSeg. alpha) ; 
delta  =  tmpSeg. end. X  *  cos (tmpSeg .alpha)  + 

tmpSeg. end. Y  *  sin (tmpSeg. alpha)  -  tmpSeg.r; 
tmpSeg. end. X  -=  delta  *  cos (tmpSeg. alpha ) ; 
tmpSeg. end. Y  -=  delta  *  sin (tmpSeg .alpha) ; 

length  =  sqrt (SQR( tmpSeg. start .X  -  tmpSeg. end. X)  +  SQR (tmpSeg. start .Y  -  tmpSeg . end. Y) ) 

seg_ptr->seg  =  tmpSeg; 
seg_ptr->length  =  length; 
seg_ptr->seg.sonarNumber  =  n; 

return  seg_ptr; 

} 
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*  Procedure;  CalculateGlobal (n) 

*  Description:  this  procedure 

*  calculates  the  global  x  and  y  coordinates  for  the  range  value  and 

*  robot  configuration  in  the  sonar  table.  The  results  are  stored  in 

*  the  sonar  table. 

void 

CalculateGlobal ( int  n) 

{ 

double  lx,  ly.  It,  range,  SonarTheta,  rob_x,  rob_y; 

CONFIGURATION  global; 

range  =  sonar_table [n] -d; 
if  (range  >=  INFINITYO)  { 

sonar_table[n] .global. X  =  INFINITY; 
sonar^t able [n] .global. Y  =  INFINITY; 

) 

else  { 

rob__x  =  sonar_table[n]  .SonarPosit  ,X; 

^oh_y  =  sonar_table[n] .SonarPosit .Y; 

SonarTheta  =  sonar_table [n] .SonarTheta;  ^ 

global  =  getRobotConf ig( ) ; 

/*  vehicle  compose  sonar  */ 

lx  =  global. Posit. X  +  (cos (global .Theta)  *  rob_x)  - 

(rob_y  *  sin (global. Theta) ) ; 
ly  =  global. Posit.Y  +  (sin (global .Theta)  *  rob_x)  + 

(rob_y  ♦  cos (global. Theta) ) ; 

It  =  SonarTheta  +  global .Theta; 

/*  vehicle  compose  sonar  range  */ 
sonar_table[n] .global. X  =  lx  +  (cos(lt)  *  range); 
sonar_t able [n] .global .Y  =  ly  +  (sin(lt)  *  range); 

} 

} 

*  Procedure:  add_to_segment (n,  x,  y)  ♦  Description:  this  procedure 

*  calculates  new  interim  data  for  the  line  segment  and  stores  it  in 

*  segment_data[n] .  It  also  changes  the  end  point  values  to  the  point 

*  being  added. 

void 

AddToSegment ( int  n,  POINT  p) 

{ 

double  mOO,  mlO,  mOl,  m20r  mil,  m02; 

double  alpha,  r; 

double  mux,  muy,  mm20,  mmll,  mm02; 

mOO  =  segment_data[n] .mOO  +=  1.0; 
mlO  =  segment_data[n]  .mlO  +=s^  p.X; 
mOl  =  segment_data[n]  .mOl  +=  p.Y; 
m20  =  segment_data[n]  .m20  ■»•=  SQR(p.X); 
mil  =  segment_data[n] .mil  +=  p.X  *  p.Y; 
m02  =  segment^data [n] .m02  +=  SQR(p.Y) ; 

if  (mOO  <  1.5) 

segment_data [n] . seg. start  =  p; 

mux  =  mlO  /  mOO; 
muy  =  mOl  /  mOO; 
mm20  =  m20  -  SQR(mlO)  /  mOO; 
mmll  =  mil  -  mlO  *  mOl  /  mOO; 
mm02  =  m02  -  SQR(mOl)  /  mOO; 
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if  (mOO  >  1.5)  C 

alpha  =  atan2(-2.0  *  mmll,  (inm02  -  mm20)  )  /  2.0; 
r  =  mux  *  cos (alpha)  +  muy  *  sin(alpha); 

segment_data[n] .seg. alpha  =  alpha; 
segment_data [n] .seg.r  =  r; 
segment_data[n] .seg. end  =  p; 

} 

} 


y ***★★*★ ★★*★★★★★★*★★*★★★★★★★★★*★★**★★★★★★★★★*★*★★*★★★*★***★★********★★★ 

*  Procedure:  reset_moments (n) ; 

*  Description:  resets  the  accumulative 

*  values  in  segment_data [n]  (m00,ml0,m01,m20,mll,m02)  to  zero. 

*  *★**★****★*★★★*★★**★**★*★★★★★*★***★*★★★★★★★•*■★★★■*■*★*★★***★*★★*****★★*★ 

void 

ResetMoments ( int  n) 

C 

segment_data [n] .mOO  =  0.0; 
segment_data [n] .mlO  =  0.0; 
segment^data [n] .mOl  =  0.0; 
segment_data [n] .m20  =  0.0; 
s  egment_dat a [ n ] . ml 1  =  0 . 0 ; 
s  egment_dat a [ n ] . mO  2  =  0.0; 

} 


/★*★★★***★★★★★★★★★**★*★*★★★★★★★★*★*★★★**★**★★**★*★********★************ 

*  Procedure:  generate_segment (n) 

*  Description:  this  function 

*  completes  segments  at  the  end  of  a  data  run.  Necessary  because  the 

*  linear  fitting  function  only  terminates  a  segment  based  on  the  data 

*  -  it  has  no  way  of  knowing  that  the  user  has  stopped  collecting  data 
★*★*★**★★★★★*★★★★★★★*★★**★★★★★★★★*★★★★★*★*****★*********************** 

void 

GenerateSegment (int  n) 

{ 

LINE_SEG  *seg_ptr; 

if  (segment_data [n] .mOO  >  10.0)  { 

seg_ptr  =  EndSegment (n) ; 

BuildList (seg_ptr,  n); 

Enqueue (n, seg_ptr) ; 

) 

ResetMoments (n) ; 


*  Procedure :  EnableLinearFitt ing (n) 

*  Description:  causes  the  background  system  to  gather  data  points 

*  from  sonar  n  and  form  them  into  line  segments  as  governed  by 

*  the  linear  fitting  algorithm. 
★★★★*★★★★*★★★★★★★★★★★★★★*★★★★★★★★★★★★★*★*★***★*********************/ 

void 

EnableLinearFitting(int  n) 

{ 


} 


sonar_table [n] . fitting  =  1; 
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/*  Procedure:  DisabieLinearFitting (n) 

*  Description:  causes  background  system  to  cease  forming  line 

*  segments  for  sonar  n. 

★****★★★*★****★★*****★★★★★**★*★*★*★★★*★*★★★★*★★*★★*★★★★★★★*★*★**★****/ 


void 

DisabieLinearFitting ( int  n) 

{ 

GenerateSegment (n) ; 
sonar_table [n] . fitting  =  0; 


*  Procedure:  LinearFitting (n) 

*  Revised  by  Y.  Kanayama, 07-07-93 

*  Description:  this  procedure  controls  the  fitting  of  point 

*  data  to  straight  line  segments.  First  it  tests  if  the  new  coming 

*  point  is  not  far  from  the  fitted  line.  If  the  test  is  passed,  the 

*  point  is  added  to  test  if  the  thinness  test  is  passed.  If  it  is 

*  passed,  the  addition  is  finalized.  If  any  of  the  tests  fail, 

*  the  line  segment  is  ended  and  a  new  one  started.  The  completed  line 

*  segment  is  stored  in  a  data  structure  called  segment,  and  segments 

*  are  linked  together  in  a  linked  list. 


void 

LinearFitting (int  n) 

{ 


POINT 

double 

double 

double 

LINE^SEG 


p; 

mOO ; 

alpha,  r,  delta; 
sonar_range ; 

*  f inished_segment ; 


sonar__range  =  sonar_table  [n]  .d; 
if  (sonar_range  <9.3  II  sonar_range  >  409.0)  { 
GenerateSegment (n) ; 
return; 


p  =  sonar_table[n] i global;  /*  temporary  moments 
mOO  =  segment_data[n] .mOO; 


if  (mOO  <  1.5)  { 

AddToSegment ( n ,  p ) ; 
return; 

} 

r  =  segment_data[n] .seg.r; 
alpha  =  segment_data[n] .seg. alpha; 

delta  =  fabs(r  -  p.X  *  cos (alpha)  -  p.Y  *  sin(alpha)); 

if  (delta  >  MAXVAL(C2,  Cl  *  sonar_range) ) 
GenerateSegment (n) ; 

AddToSegment (n,  p)  ; 
return; 

}  /*  end  linear_fitting  */ 
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/ 

*  Procedure:  build_list (ptr,  n) ; 

*  Description:  this  function  accepts 

*  a  pointer  to  a  segment  data  structure  and  a  sonar  number,  and 

*  appends  the  segment  structure  to  the  tail  of  a  linked  list  of 

*  structures  for  that  sonar, 

★★**★★*★***★*★*★*★★*★★★*★★★•*★★*★★*★★★*★★★★★*★***★★*****★********** 

void 

BuildList (LINE^SEG  *ptr,  int  n) 

{ 

int  next ; 

if  (SegListTail [n]  ==  -1) 

SegListHead [n]  =  0; 

next  =  (SegListTail [n]  <  4)  ?  ++SegListTail [n]  ;  0; 

if  (next  ==  SegListHead [n] ) 

SegListHead [n]  =  (SegListHead [n]  <  4)  ?  ++SegListHead [n]  :  0 

seg_list [n] [next ]  =  *ptr; 

LogSonarSegmentData (n,  seg_list[n] [next]) ; 

) 

*  Procedure:  ScanSonar ( int  dir,  double  dist) 

*  Description:  Function  allows  user  to  scan  in  one  of  four 

*  directions  for  obstacles.  Function  will  return 

*  when  it  detects  an  obstacles  within  the  specified 

*  distance.  Default  is  forward  scan. 

*  Inputs:  Scan  Direction  and  detection  distance 

*  Outputs:  Global  coordinates  of  obstacle 

*  Date:  06  DEC  94 

*  Author:  LCDR  Jane  Lochner 

POINT 

ScanSonar (int  dir, double  dist) 

{ 

double  hitl=9999.9,hit2=9999.9,hit3=9999.9; 

POINT  obstacle; 

int  sonar 1, sonar2 , sonar3 ; 

switch(dir)  { 
case  FORWARD: 

‘Sonarl=S330 ; 
sonar2=S000 ; 
sonar3=S030  ; 
break; 

case  BACKWARD: 
sonarl=S210 ; 
sonar2=S180 ; 
sonar3=S150 ; 
break; 
case  LEFT: 
sonarl=S300 ; 
sonar2=S270 ; 
sonar3=S240 ; 
break; 
case  RIGHT: 
sonarl=S060 ; 
sonar2=S090 ; 
sonar3=S120 ; 
default : 

sonarl=S330 ; 
sonar2=S000; 

sonar3=S030;  go 

break; 


} 


EnableSonar (sonarl) ; 
EnableSonar ( sonar2 ) ; 
EnableSonar (sonar3) ; 
do  { 


waitMS  (30)  ; 

hi t l=Sonar ( sonarl ) 

waitMS (30) ; 


hit2=Sonar (sonar2) ; 

waitMS (30)  ; 

hit 3  =Sonar ( sonar3 ) ; 

}  while  ( (hitl>dist  II  hitl<1.0)  && 
(hit2>dist  I  I  hit2<1.0)  && 
(hit3>dist 1  I  hit3<l . 0) ) ; 


if  (hitl<dist) 

obstaclesGlobal (sonarl) ; 
if  (hit2<dist) 

obstacle=Global (sonar2 ) ; 
if  (hit3<dist) 

obstacle=Global (sonar3 ) ; 


DisableSonar (sonarl) ; 
DisableSonar (sonar2) ; 
DisableSonar (sonar3) ; 


) 


return (obstacle) ; 


y*iHHtit*itiHHt**it*’iriitit****itilr**itiHtitilt*it***‘^t*ilritit****ilt*-ir***it********** 

*  Procedure:  get Boundaries (POINT  scan) 

*  Description:  Function  returns  the  left  and  right 

*  boundaries  of  an  object.  This  is  a 

*  dummy  function  to  be  iitqplemented  at 

*  a  later  date.  The  user  just  inputs 

*  values  to  be  returned. 

*  Date:  06  DEC  94 

^  Author:  LCDR  Jane  Lochner 

ie  Hr  **  ir  *  it  *****  it  ieit  Hr  *************  it  it**  f 

BOUNDARY 

getBoundaries (POINT  scan) 

{ 

BOUNDARY  obstacle; 

obstacle. left .Y  =  105.0; 
obstacle. left .X  =  150.0; 
obstacle.right .Y  =  25.0; 
obstacle.right .X  =  150.0; 

return (obstacle) ; 

) 
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/★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★♦★★★********-^*-*’*****'*^******* 

*  Procedure:  getWidth  (POINT  scan,int  direction) 

*  Description:  Function  returns  the  width  of  object 

*  on  the  designated  side.  This  is  a 

*  dummy  function  to  be  implemented  at 

*  a  later  date.  The  user  just  inputs 

*  the  values  for  testing  purposes. 

*  Date:  06  DEC  94 

*  Author:  LCDR  Jane  Lochner 

double 

getWidth (POINT  scan, int  direction) 

{ 

double  width; 

if  (direction  -=  LEFT) 
width  =  80.0; 
else 

width  =  -55.0; 
return  (width) ; 

} 

/****★★★**★**★**★★★★★**★*★★★*★*★**★*★★*******★*★★★★★*****★*★** 

*  Procedure:  avoidPathVert ex (double  safety,  POINT  scan) 

*  Description:  Calculates  path  to  avoid  obstacle 

*  using  designated  safety  margin  and  the 

*  outer  vertices  of  the  object 

*  Inputs:  safety  margin,  global  coordinates  of  closest 

*  point 

*  Outputs :  New  path  to  avoid  obstacle 

*  Date:  06  DEC  94 

*  Author:  LCDR  Jane  Lochner 

CONFIGURATION 

avoidPathVert ex (double  safety,  POINT  scan) 

{ 

CONFIGURATION  Current , avoid; 

POINT  left, right ; 

BOUNDARY  obstacle; 
double  LeftDist , RightDist ; 

Current  =  getRobotConf ig ( ) ; 
obstacle  =  getBoundaries ( scan) ; 

LeftDist  =  - (obstacle . left .X-Current . Posit .X) *sin (Current .Theta) + 

(obstacle . lef t . Y-Current . Posit . Y) *cos (Current .Theta) +saf ety ; 
RightDist  =  - (obstacle . right .X-Current . Posit .X) * sin (Current .Theta) + 

(obstacle . right .Y-Current . Posit .Y) *cos (Current .Theta) -safety 

left.X  =  Current  .Posit  .X  -  Lef  tDist sin  (Current  .Theta)  ; 
left.Y  =  Current .Posit .Y  +  Lef tDist*cos (Current .Theta) ; 
right. X  =  Current . Posit .X  -  RightDist *sin (Current .Theta) ; 
right. Y  =  Current . Posit .Y  +  RightDist*cos (Current .Theta); 


if  (LeftDist  >  -RightDist)  { 

avoid. Posit .X  =  right. X; 
avoid. Posit .Y  =  right.Y;} 

else  { 

avoid. Posit .X  =  left.X; 
avoid. Posit .Y  =  left.Y;} 
avoid. Theta  =  Current .Theta; 
avoid. Kappa  =  Current .Kappa ; 
return (avoid) ; 

} 
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*  Procedure:  avoidPathWidth (double  safety,  POINT  scan) 

*  Description:  Calculates  path  to  avoid  obstacle 

*  using  designated  safety  margin  and  the 
left  and  right  widths  of  the  object 

Inputs:  safety  margin,  global  coordinates  of  closest 
point 

*  Outputs:  New  path  to  avoid  obstacle 

^  Date:  06  DEC  94 

^  Author:  LCDR  Jane  Lochner 

************************♦***•**•**********»****************/ 

CONFIGURATION 

avoidPathWidth (double  safety,  POINT  scan) 

{ 

CONFIGURATION  Current , avoid; 

POINT  left, right; 
double  LeftDist,RightDist; 

Current  =  getRobotConfig( ) ; 

LeftDist  =  getWidth (scan, LEFT) ; 

Ri ghtD i s  t  =  getWidth ( s can , RIGHT ) ; 

left.X  =  Current. Pos it. X  -  LeftDist*sin(Current .Theta) ; 
left.Y  =  Cur rent. Pos it.Y  +  LeftDist*cos (Current .Theta) ; 
right. X  =  Current. Pos it. X  -  RightDist*sin(Current. Theta) ; 
right. Y  =  Current .Posit .Y  +  RightDist*cos (Current .Theta) ; 


if  (LeftDist  >  -RightDist) 

{ 

avoid.Posit.X  =  right.X; 
avoid. Posit. Y  *  right. Y; 


avoid.Posit.X  =  left.X; 
avoid. Posit. Y  =  left.Y; 

) 

avoid. Theta  =  Current .Theta ; 
a VO id. Kappa  =  Current . Kappa ; 
return (avoid) ; 

) 
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